git commit -m "first commit"
This commit is contained in:
62
navigations/dwb_local_planner/.$structure.drawio.bkp
Executable file
62
navigations/dwb_local_planner/.$structure.drawio.bkp
Executable file
@@ -0,0 +1,62 @@
|
||||
<mxfile host="Electron" modified="2023-09-19T03:46:50.690Z" agent="Mozilla/5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/21.7.5 Chrome/114.0.5735.289 Electron/25.8.1 Safari/537.36" etag="N-SpsWCoSkmju6tHr8Jh" version="21.7.5" type="device">
|
||||
<diagram name="Page-1" id="2YBvvXClWsGukQMizWep">
|
||||
<mxGraphModel dx="519" dy="1179" grid="1" gridSize="10" guides="1" tooltips="1" connect="1" arrows="1" fold="1" page="1" pageScale="1" pageWidth="850" pageHeight="1100" math="0" shadow="0">
|
||||
<root>
|
||||
<mxCell id="0" />
|
||||
<mxCell id="1" parent="0" />
|
||||
<mxCell id="aM9ryv3xv72pqoxQDRHE-1" value="nav_core_adapter::<br>LocalPlannerAdapter" style="shape=umlLifeline;perimeter=lifelinePerimeter;whiteSpace=wrap;html=1;container=0;dropTarget=0;collapsible=0;recursiveResize=0;outlineConnect=0;portConstraint=eastwest;newEdgeStyle={"edgeStyle":"elbowEdgeStyle","elbow":"vertical","curved":0,"rounded":0};" parent="1" vertex="1">
|
||||
<mxGeometry x="190" y="110" width="180" height="360" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="aM9ryv3xv72pqoxQDRHE-2" value="" style="html=1;points=[];perimeter=orthogonalPerimeter;outlineConnect=0;targetShapes=umlLifeline;portConstraint=eastwest;newEdgeStyle={"edgeStyle":"elbowEdgeStyle","elbow":"vertical","curved":0,"rounded":0};" parent="aM9ryv3xv72pqoxQDRHE-1" vertex="1">
|
||||
<mxGeometry x="45" y="70" width="10" height="190" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="aM9ryv3xv72pqoxQDRHE-3" value="dispatch" style="html=1;verticalAlign=bottom;startArrow=oval;endArrow=block;startSize=8;edgeStyle=elbowEdgeStyle;elbow=vertical;curved=0;rounded=0;" parent="aM9ryv3xv72pqoxQDRHE-1" target="aM9ryv3xv72pqoxQDRHE-2" edge="1">
|
||||
<mxGeometry relative="1" as="geometry">
|
||||
<mxPoint x="-15" y="70" as="sourcePoint" />
|
||||
</mxGeometry>
|
||||
</mxCell>
|
||||
<mxCell id="aM9ryv3xv72pqoxQDRHE-4" value="" style="html=1;points=[];perimeter=orthogonalPerimeter;outlineConnect=0;targetShapes=umlLifeline;portConstraint=eastwest;newEdgeStyle={"edgeStyle":"elbowEdgeStyle","elbow":"vertical","curved":0,"rounded":0};" parent="aM9ryv3xv72pqoxQDRHE-1" vertex="1">
|
||||
<mxGeometry x="50" y="120" width="10" height="80" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="aM9ryv3xv72pqoxQDRHE-5" value=":Object" style="shape=umlLifeline;perimeter=lifelinePerimeter;whiteSpace=wrap;html=1;container=0;dropTarget=0;collapsible=0;recursiveResize=0;outlineConnect=0;portConstraint=eastwest;newEdgeStyle={"edgeStyle":"elbowEdgeStyle","elbow":"vertical","curved":0,"rounded":0};" parent="1" vertex="1">
|
||||
<mxGeometry x="473" y="110" width="100" height="300" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="aM9ryv3xv72pqoxQDRHE-6" value="" style="html=1;points=[];perimeter=orthogonalPerimeter;outlineConnect=0;targetShapes=umlLifeline;portConstraint=eastwest;newEdgeStyle={"edgeStyle":"elbowEdgeStyle","elbow":"vertical","curved":0,"rounded":0};" parent="aM9ryv3xv72pqoxQDRHE-5" vertex="1">
|
||||
<mxGeometry x="45" y="80" width="10" height="170" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="aM9ryv3xv72pqoxQDRHE-7" value="dispatch" style="html=1;verticalAlign=bottom;endArrow=block;edgeStyle=elbowEdgeStyle;elbow=vertical;curved=0;rounded=0;" parent="1" source="aM9ryv3xv72pqoxQDRHE-2" target="aM9ryv3xv72pqoxQDRHE-6" edge="1">
|
||||
<mxGeometry relative="1" as="geometry">
|
||||
<mxPoint x="448" y="270" as="sourcePoint" />
|
||||
<Array as="points">
|
||||
<mxPoint x="433" y="260" />
|
||||
</Array>
|
||||
</mxGeometry>
|
||||
</mxCell>
|
||||
<mxCell id="aM9ryv3xv72pqoxQDRHE-8" value="return" style="html=1;verticalAlign=bottom;endArrow=open;dashed=1;endSize=8;edgeStyle=elbowEdgeStyle;elbow=vertical;curved=0;rounded=0;" parent="1" source="aM9ryv3xv72pqoxQDRHE-6" target="aM9ryv3xv72pqoxQDRHE-2" edge="1">
|
||||
<mxGeometry relative="1" as="geometry">
|
||||
<mxPoint x="448" y="345" as="targetPoint" />
|
||||
<Array as="points">
|
||||
<mxPoint x="443" y="420" />
|
||||
</Array>
|
||||
</mxGeometry>
|
||||
</mxCell>
|
||||
<mxCell id="aM9ryv3xv72pqoxQDRHE-9" value="callback" style="html=1;verticalAlign=bottom;endArrow=block;edgeStyle=elbowEdgeStyle;elbow=vertical;curved=0;rounded=0;" parent="1" source="aM9ryv3xv72pqoxQDRHE-6" target="aM9ryv3xv72pqoxQDRHE-4" edge="1">
|
||||
<mxGeometry relative="1" as="geometry">
|
||||
<mxPoint x="428" y="300" as="sourcePoint" />
|
||||
<Array as="points">
|
||||
<mxPoint x="443" y="300" />
|
||||
</Array>
|
||||
</mxGeometry>
|
||||
</mxCell>
|
||||
<mxCell id="aM9ryv3xv72pqoxQDRHE-10" value="return" style="html=1;verticalAlign=bottom;endArrow=open;dashed=1;endSize=8;edgeStyle=elbowEdgeStyle;elbow=vertical;curved=0;rounded=0;" parent="1" source="aM9ryv3xv72pqoxQDRHE-4" target="aM9ryv3xv72pqoxQDRHE-6" edge="1">
|
||||
<mxGeometry relative="1" as="geometry">
|
||||
<mxPoint x="428" y="375" as="targetPoint" />
|
||||
<Array as="points">
|
||||
<mxPoint x="433" y="370" />
|
||||
</Array>
|
||||
</mxGeometry>
|
||||
</mxCell>
|
||||
</root>
|
||||
</mxGraphModel>
|
||||
</diagram>
|
||||
</mxfile>
|
||||
152
navigations/dwb_local_planner/.$structure.drawio.dtmp
Executable file
152
navigations/dwb_local_planner/.$structure.drawio.dtmp
Executable file
@@ -0,0 +1,152 @@
|
||||
<mxfile host="Electron" modified="2023-09-19T06:48:28.764Z" agent="Mozilla/5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/21.7.5 Chrome/114.0.5735.289 Electron/25.8.1 Safari/537.36" etag="oQzu2aORHDBJaUZ2wsCe" version="21.7.5" type="device">
|
||||
<diagram name="Page-1" id="2YBvvXClWsGukQMizWep">
|
||||
<mxGraphModel dx="818" dy="1938" grid="1" gridSize="10" guides="1" tooltips="1" connect="1" arrows="1" fold="1" page="1" pageScale="1" pageWidth="850" pageHeight="1100" math="0" shadow="0">
|
||||
<root>
|
||||
<mxCell id="0" />
|
||||
<mxCell id="1" parent="0" />
|
||||
<mxCell id="aM9ryv3xv72pqoxQDRHE-1" value="<div style="color: rgb(246, 246, 244); background-color: rgb(40, 42, 54); font-family: &quot;Droid Sans Mono&quot;, &quot;monospace&quot;, monospace; font-weight: normal; line-height: 19px;"><div style=""><span style="color: rgb(151, 225, 241); font-style: italic;"><font style="font-size: 10px;">LocalPlannerAdapter</font></span></div></div>" style="shape=umlLifeline;perimeter=lifelinePerimeter;whiteSpace=wrap;html=1;container=0;dropTarget=0;collapsible=0;recursiveResize=0;outlineConnect=0;portConstraint=eastwest;newEdgeStyle={"edgeStyle":"elbowEdgeStyle","elbow":"vertical","curved":0,"rounded":0};" parent="1" vertex="1">
|
||||
<mxGeometry x="230" y="110" width="140" height="780" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="aM9ryv3xv72pqoxQDRHE-2" value="" style="html=1;points=[];perimeter=orthogonalPerimeter;outlineConnect=0;targetShapes=umlLifeline;portConstraint=eastwest;newEdgeStyle={"edgeStyle":"elbowEdgeStyle","elbow":"vertical","curved":0,"rounded":0};" parent="aM9ryv3xv72pqoxQDRHE-1" vertex="1">
|
||||
<mxGeometry x="45" y="70" width="10" height="700" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="aM9ryv3xv72pqoxQDRHE-3" value="<div style="color: rgb(246, 246, 244); background-color: rgb(40, 42, 54); font-family: &quot;Droid Sans Mono&quot;, &quot;monospace&quot;, monospace; line-height: 19px;"><font style="font-size: 10px;">geometry_msgs::<span style="color: rgb(151, 225, 241); font-style: italic;">Twist</span>&nbsp;</font></div><div style="color: rgb(246, 246, 244); background-color: rgb(40, 42, 54); font-family: &quot;Droid Sans Mono&quot;, &quot;monospace&quot;, monospace; line-height: 19px;"><font style="font-size: 10px;"><span style="color: rgb(255, 184, 108); font-style: italic;">cmd_vel</span></font></div>" style="html=1;verticalAlign=bottom;startArrow=oval;endArrow=block;startSize=8;edgeStyle=elbowEdgeStyle;elbow=vertical;curved=0;rounded=0;" parent="aM9ryv3xv72pqoxQDRHE-1" target="aM9ryv3xv72pqoxQDRHE-2" edge="1">
|
||||
<mxGeometry x="-0.8788" y="10" relative="1" as="geometry">
|
||||
<mxPoint x="-120" y="70" as="sourcePoint" />
|
||||
<mxPoint as="offset" />
|
||||
</mxGeometry>
|
||||
</mxCell>
|
||||
<mxCell id="A1EfnVpFrXilNoYTl5xU-12" value="" style="html=1;points=[[0,0,0,0,5],[0,1,0,0,-5],[1,0,0,0,5],[1,1,0,0,-5]];perimeter=orthogonalPerimeter;outlineConnect=0;targetShapes=umlLifeline;portConstraint=eastwest;newEdgeStyle={"curved":0,"rounded":0};fillColor=#60a917;fontColor=#ffffff;strokeColor=#2D7600;" vertex="1" parent="aM9ryv3xv72pqoxQDRHE-1">
|
||||
<mxGeometry x="45" y="115" width="10" height="575" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="aM9ryv3xv72pqoxQDRHE-5" value="<div style="color: rgb(246, 246, 244); background-color: rgb(40, 42, 54); font-family: &quot;Droid Sans Mono&quot;, &quot;monospace&quot;, monospace; font-weight: normal; line-height: 19px;"><div style=""><span style="color: rgb(151, 225, 241); font-style: italic;"><font style="font-size: 10px;">DWBLocalPlanner</font></span></div></div>" style="shape=umlLifeline;perimeter=lifelinePerimeter;whiteSpace=wrap;html=1;container=0;dropTarget=0;collapsible=0;recursiveResize=0;outlineConnect=0;portConstraint=eastwest;newEdgeStyle={"edgeStyle":"elbowEdgeStyle","elbow":"vertical","curved":0,"rounded":0};" parent="1" vertex="1">
|
||||
<mxGeometry x="480" y="190" width="100" height="1320" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="aM9ryv3xv72pqoxQDRHE-6" value="" style="html=1;points=[];perimeter=orthogonalPerimeter;outlineConnect=0;targetShapes=umlLifeline;portConstraint=eastwest;newEdgeStyle={"edgeStyle":"elbowEdgeStyle","elbow":"vertical","curved":0,"rounded":0};" parent="aM9ryv3xv72pqoxQDRHE-5" vertex="1">
|
||||
<mxGeometry x="45" y="80" width="10" height="1240" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="A1EfnVpFrXilNoYTl5xU-20" value="" style="html=1;points=[[0,0,0,0,5],[0,1,0,0,-5],[1,0,0,0,5],[1,1,0,0,-5]];perimeter=orthogonalPerimeter;outlineConnect=0;targetShapes=umlLifeline;portConstraint=eastwest;newEdgeStyle={"curved":0,"rounded":0};fillColor=#60a917;fontColor=#ffffff;strokeColor=#2D7600;" vertex="1" parent="aM9ryv3xv72pqoxQDRHE-5">
|
||||
<mxGeometry x="45" y="110" width="10" height="440" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="A1EfnVpFrXilNoYTl5xU-3" value="" style="endArrow=block;startArrow=block;endFill=1;startFill=1;html=1;rounded=0;" edge="1" parent="1" target="A1EfnVpFrXilNoYTl5xU-52">
|
||||
<mxGeometry width="160" relative="1" as="geometry">
|
||||
<mxPoint x="284.9999999999999" y="760" as="sourcePoint" />
|
||||
<mxPoint x="790" y="760" as="targetPoint" />
|
||||
</mxGeometry>
|
||||
</mxCell>
|
||||
<mxCell id="A1EfnVpFrXilNoYTl5xU-18" value="<div style="color: rgb(246, 246, 244); background-color: rgb(40, 42, 54); font-family: &quot;Droid Sans Mono&quot;, &quot;monospace&quot;, monospace; font-weight: normal; line-height: 19px;"><div style=""><font style="font-size: 10px;"><span style="color: rgb(246, 246, 244);">geometry_msgs</span><span style="color: rgb(242, 134, 196);">::</span><span style="color: rgb(151, 225, 241); font-style: italic;">Twist</span><span style="color: rgb(242, 134, 196);">&amp;</span><span style="color: rgb(246, 246, 244);">&nbsp;</span></font></div><div style=""><font style="font-size: 10px;"><span style="color: rgb(255, 184, 108); font-style: italic;">cmd_vel</span></font></div></div>" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" vertex="1" connectable="0" parent="A1EfnVpFrXilNoYTl5xU-3">
|
||||
<mxGeometry x="0.0099" relative="1" as="geometry">
|
||||
<mxPoint x="-79" y="-30" as="offset" />
|
||||
</mxGeometry>
|
||||
</mxCell>
|
||||
<mxCell id="A1EfnVpFrXilNoYTl5xU-10" value="return T/F" style="html=1;verticalAlign=bottom;endArrow=open;dashed=1;endSize=8;curved=0;rounded=0;" edge="1" parent="1">
|
||||
<mxGeometry relative="1" as="geometry">
|
||||
<mxPoint x="520" y="830" as="sourcePoint" />
|
||||
<mxPoint x="288" y="830" as="targetPoint" />
|
||||
</mxGeometry>
|
||||
</mxCell>
|
||||
<mxCell id="A1EfnVpFrXilNoYTl5xU-13" value="<div style="color: rgb(246, 246, 244); background-color: rgb(40, 42, 54); font-family: &quot;Droid Sans Mono&quot;, &quot;monospace&quot;, monospace; line-height: 19px;"><br></div>" style="html=1;verticalAlign=bottom;startArrow=oval;endArrow=block;startSize=8;curved=0;rounded=0;entryX=0;entryY=0;entryDx=0;entryDy=5;entryPerimeter=0;" edge="1" target="A1EfnVpFrXilNoYTl5xU-12" parent="1">
|
||||
<mxGeometry relative="1" as="geometry">
|
||||
<mxPoint x="110" y="230" as="sourcePoint" />
|
||||
</mxGeometry>
|
||||
</mxCell>
|
||||
<mxCell id="A1EfnVpFrXilNoYTl5xU-17" value="<div style="color: rgb(246, 246, 244); background-color: rgb(40, 42, 54); font-family: &quot;Droid Sans Mono&quot;, &quot;monospace&quot;, monospace; font-weight: normal; line-height: 19px;"><div style=""><span style="color: rgb(98, 232, 132);"><font style="font-size: 10px;">computeVelocityCommands</font></span></div></div>" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" vertex="1" connectable="0" parent="A1EfnVpFrXilNoYTl5xU-13">
|
||||
<mxGeometry x="0.004" y="1" relative="1" as="geometry">
|
||||
<mxPoint x="-32" y="-19" as="offset" />
|
||||
</mxGeometry>
|
||||
</mxCell>
|
||||
<mxCell id="A1EfnVpFrXilNoYTl5xU-23" value="" style="html=1;verticalAlign=bottom;endArrow=open;dashed=1;endSize=8;curved=0;rounded=0;" edge="1" parent="1" source="A1EfnVpFrXilNoYTl5xU-43" target="A1EfnVpFrXilNoYTl5xU-20">
|
||||
<mxGeometry relative="1" as="geometry">
|
||||
<mxPoint x="740" y="920" as="sourcePoint" />
|
||||
<mxPoint x="500" y="920" as="targetPoint" />
|
||||
</mxGeometry>
|
||||
</mxCell>
|
||||
<mxCell id="A1EfnVpFrXilNoYTl5xU-51" value="<div style="color: rgb(246, 246, 244); background-color: rgb(40, 42, 54); font-family: &quot;Droid Sans Mono&quot;, &quot;monospace&quot;, monospace; font-weight: normal; line-height: 19px;"><div style=""><font style="font-size: 10px;"><span style="color: rgb(246, 246, 244);">nav_2d_msgs</span><span style="color: rgb(242, 134, 196);">::</span><span style="color: rgb(151, 225, 241); font-style: italic;">Twist2D</span><span style="color: rgb(246, 246, 244);"> </span><span style="color: rgb(246, 246, 244);">velocity</span></font></div></div>" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" vertex="1" connectable="0" parent="A1EfnVpFrXilNoYTl5xU-23">
|
||||
<mxGeometry x="-0.0222" relative="1" as="geometry">
|
||||
<mxPoint y="-15" as="offset" />
|
||||
</mxGeometry>
|
||||
</mxCell>
|
||||
<mxCell id="A1EfnVpFrXilNoYTl5xU-29" value="" style="html=1;verticalAlign=bottom;startArrow=oval;startFill=1;endArrow=block;startSize=8;curved=0;rounded=0;" edge="1" parent="1">
|
||||
<mxGeometry width="60" relative="1" as="geometry">
|
||||
<mxPoint x="340" y="310" as="sourcePoint" />
|
||||
<mxPoint x="525" y="310" as="targetPoint" />
|
||||
</mxGeometry>
|
||||
</mxCell>
|
||||
<mxCell id="A1EfnVpFrXilNoYTl5xU-32" value="<span style="color: rgb(98, 232, 132); font-family: &quot;Droid Sans Mono&quot;, &quot;monospace&quot;, monospace; font-size: 10px; font-style: normal; font-variant-ligatures: normal; font-variant-caps: normal; font-weight: 400; letter-spacing: normal; orphans: 2; text-align: center; text-indent: 0px; text-transform: none; widows: 2; word-spacing: 0px; -webkit-text-stroke-width: 0px; background-color: rgb(40, 42, 54); text-decoration-thickness: initial; text-decoration-style: initial; text-decoration-color: initial; float: none; display: inline !important;">computeVelocityCommands</span>" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" vertex="1" connectable="0" parent="A1EfnVpFrXilNoYTl5xU-29">
|
||||
<mxGeometry x="-0.174" y="2" relative="1" as="geometry">
|
||||
<mxPoint x="-17" y="-18" as="offset" />
|
||||
</mxGeometry>
|
||||
</mxCell>
|
||||
<mxCell id="A1EfnVpFrXilNoYTl5xU-31" value="<div style="color: rgb(246, 246, 244); background-color: rgb(40, 42, 54); font-family: &quot;Droid Sans Mono&quot;, &quot;monospace&quot;, monospace; font-weight: normal; line-height: 19px;"><div style=""><br></div></div>" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" vertex="1" connectable="0" parent="1">
|
||||
<mxGeometry x="400" y="250" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="A1EfnVpFrXilNoYTl5xU-34" value="" style="html=1;verticalAlign=bottom;startArrow=circle;startFill=1;endArrow=open;startSize=6;endSize=8;curved=0;rounded=0;" edge="1" parent="1" target="A1EfnVpFrXilNoYTl5xU-42">
|
||||
<mxGeometry width="80" relative="1" as="geometry">
|
||||
<mxPoint x="640" y="270" as="sourcePoint" />
|
||||
<mxPoint x="770" y="270" as="targetPoint" />
|
||||
</mxGeometry>
|
||||
</mxCell>
|
||||
<mxCell id="A1EfnVpFrXilNoYTl5xU-35" value="<div style="color: rgb(246, 246, 244); background-color: rgb(40, 42, 54); font-family: &quot;Droid Sans Mono&quot;, &quot;monospace&quot;, monospace; font-weight: normal; line-height: 19px;"><div style=""><span style="color: rgb(98, 232, 132);"><font style="font-size: 10px;">getRobotPose</font></span></div></div>" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" vertex="1" connectable="0" parent="A1EfnVpFrXilNoYTl5xU-34">
|
||||
<mxGeometry x="-0.0225" relative="1" as="geometry">
|
||||
<mxPoint x="-10" y="-20" as="offset" />
|
||||
</mxGeometry>
|
||||
</mxCell>
|
||||
<mxCell id="A1EfnVpFrXilNoYTl5xU-40" value="" style="endArrow=block;startArrow=block;endFill=1;startFill=1;html=1;rounded=0;" edge="1" parent="1">
|
||||
<mxGeometry width="160" relative="1" as="geometry">
|
||||
<mxPoint x="750" y="335" as="sourcePoint" />
|
||||
<mxPoint x="535" y="335" as="targetPoint" />
|
||||
</mxGeometry>
|
||||
</mxCell>
|
||||
<mxCell id="A1EfnVpFrXilNoYTl5xU-41" value="<div style="color: rgb(246, 246, 244); background-color: rgb(40, 42, 54); font-family: &quot;Droid Sans Mono&quot;, &quot;monospace&quot;, monospace; font-weight: normal; line-height: 19px;"><div style=""><font style="font-size: 10px;"><span style="color: rgb(246, 246, 244);">nav_2d_msgs</span><span style="color: rgb(242, 134, 196);">::</span><span style="color: rgb(151, 225, 241); font-style: italic;">Pose2DStamped</span><span style="color: rgb(246, 246, 244);">&nbsp;</span></font></div><div style=""><font style="font-size: 10px;"><span style="color: rgb(246, 246, 244);">&amp; pose2d</span></font></div></div>" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" vertex="1" connectable="0" parent="A1EfnVpFrXilNoYTl5xU-40">
|
||||
<mxGeometry x="0.1329" relative="1" as="geometry">
|
||||
<mxPoint x="16" y="-25" as="offset" />
|
||||
</mxGeometry>
|
||||
</mxCell>
|
||||
<mxCell id="A1EfnVpFrXilNoYTl5xU-42" value="" style="html=1;points=[[0,0,0,0,5],[0,1,0,0,-5],[1,0,0,0,5],[1,1,0,0,-5]];perimeter=orthogonalPerimeter;outlineConnect=0;targetShapes=umlLifeline;portConstraint=eastwest;newEdgeStyle={"curved":0,"rounded":0};fillColor=#60a917;fontColor=#ffffff;strokeColor=#2D7600;" vertex="1" parent="1">
|
||||
<mxGeometry x="750" y="250" width="10" height="105" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="A1EfnVpFrXilNoYTl5xU-46" value="<div style="color: rgb(246, 246, 244); background-color: rgb(40, 42, 54); font-family: &quot;Droid Sans Mono&quot;, &quot;monospace&quot;, monospace; font-weight: normal; line-height: 19px;"><div style="">nav_2d_utils<span style="font-size: 10px; color: rgb(242, 134, 196); text-align: start;" class="mtk10">::<br></span><span style="font-size: 10px; text-align: start;" class="mtk1">OdomSubscriber</span><br></div></div>" style="shape=umlLifeline;perimeter=lifelinePerimeter;whiteSpace=wrap;html=1;container=0;dropTarget=0;collapsible=0;recursiveResize=0;outlineConnect=0;portConstraint=eastwest;newEdgeStyle={"edgeStyle":"elbowEdgeStyle","elbow":"vertical","curved":0,"rounded":0};" vertex="1" parent="1">
|
||||
<mxGeometry x="705" y="370" width="100" height="180" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="A1EfnVpFrXilNoYTl5xU-47" value="" style="html=1;points=[];perimeter=orthogonalPerimeter;outlineConnect=0;targetShapes=umlLifeline;portConstraint=eastwest;newEdgeStyle={"edgeStyle":"elbowEdgeStyle","elbow":"vertical","curved":0,"rounded":0};" vertex="1" parent="A1EfnVpFrXilNoYTl5xU-46">
|
||||
<mxGeometry x="45" y="80" width="10" height="130" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="A1EfnVpFrXilNoYTl5xU-43" value="" style="html=1;points=[[0,0,0,0,5],[0,1,0,0,-5],[1,0,0,0,5],[1,1,0,0,-5]];perimeter=orthogonalPerimeter;outlineConnect=0;targetShapes=umlLifeline;portConstraint=eastwest;newEdgeStyle={"curved":0,"rounded":0};fillColor=#60a917;fontColor=#ffffff;strokeColor=#2D7600;" vertex="1" parent="A1EfnVpFrXilNoYTl5xU-46">
|
||||
<mxGeometry x="45" y="80" width="10" height="130" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="A1EfnVpFrXilNoYTl5xU-49" value="" style="html=1;verticalAlign=bottom;startArrow=circle;startFill=1;endArrow=open;startSize=6;endSize=8;curved=0;rounded=0;" edge="1" parent="1" target="A1EfnVpFrXilNoYTl5xU-43">
|
||||
<mxGeometry width="80" relative="1" as="geometry">
|
||||
<mxPoint x="630" y="470" as="sourcePoint" />
|
||||
<mxPoint x="740" y="470" as="targetPoint" />
|
||||
</mxGeometry>
|
||||
</mxCell>
|
||||
<mxCell id="A1EfnVpFrXilNoYTl5xU-50" value="<div style="color: rgb(246, 246, 244); background-color: rgb(40, 42, 54); font-family: &quot;Droid Sans Mono&quot;, &quot;monospace&quot;, monospace; font-weight: normal; line-height: 19px;"><div style=""><span style="color: rgb(98, 232, 132);"><font style="font-size: 10px;">getTwist</font></span></div></div>" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" vertex="1" connectable="0" parent="A1EfnVpFrXilNoYTl5xU-49">
|
||||
<mxGeometry x="-0.0225" relative="1" as="geometry">
|
||||
<mxPoint x="-10" y="-20" as="offset" />
|
||||
</mxGeometry>
|
||||
</mxCell>
|
||||
<mxCell id="A1EfnVpFrXilNoYTl5xU-52" value="" style="html=1;points=[[0,0,0,0,5],[0,1,0,0,-5],[1,0,0,0,5],[1,1,0,0,-5]];perimeter=orthogonalPerimeter;outlineConnect=0;targetShapes=umlLifeline;portConstraint=eastwest;newEdgeStyle={"curved":0,"rounded":0};fillColor=#60a917;fontColor=#ffffff;strokeColor=#2D7600;" vertex="1" parent="1">
|
||||
<mxGeometry x="750" y="600" width="10" height="230" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="A1EfnVpFrXilNoYTl5xU-53" value="<div style="color: rgb(246, 246, 244); background-color: rgb(40, 42, 54); font-family: &quot;Droid Sans Mono&quot;, &quot;monospace&quot;, monospace; line-height: 19px;"><font style="font-size: 10px;">nav_2d_msgs::<span style="color: rgb(151, 225, 241); font-style: italic;">Twist2DStamped</span>&nbsp;</font></div><div style="color: rgb(246, 246, 244); background-color: rgb(40, 42, 54); font-family: &quot;Droid Sans Mono&quot;, &quot;monospace&quot;, monospace; line-height: 19px;"><font style="font-size: 10px;">cmd_vel_2d</font></div>" style="html=1;verticalAlign=bottom;endArrow=open;dashed=1;endSize=8;curved=0;rounded=0;" edge="1" parent="1">
|
||||
<mxGeometry x="0.3488" relative="1" as="geometry">
|
||||
<mxPoint x="535" y="710" as="sourcePoint" />
|
||||
<mxPoint x="750" y="710" as="targetPoint" />
|
||||
<mxPoint as="offset" />
|
||||
</mxGeometry>
|
||||
</mxCell>
|
||||
<mxCell id="A1EfnVpFrXilNoYTl5xU-55" value="" style="html=1;verticalAlign=bottom;startArrow=oval;startFill=1;endArrow=block;startSize=8;curved=0;rounded=0;entryX=0;entryY=0;entryDx=0;entryDy=5;entryPerimeter=0;" edge="1" parent="1" target="A1EfnVpFrXilNoYTl5xU-52">
|
||||
<mxGeometry width="60" relative="1" as="geometry">
|
||||
<mxPoint x="600" y="605" as="sourcePoint" />
|
||||
<mxPoint x="455" y="540" as="targetPoint" />
|
||||
</mxGeometry>
|
||||
</mxCell>
|
||||
<mxCell id="A1EfnVpFrXilNoYTl5xU-56" value="<div style="color: rgb(246, 246, 244); background-color: rgb(40, 42, 54); font-family: &quot;Droid Sans Mono&quot;, &quot;monospace&quot;, monospace; font-weight: normal; line-height: 19px;"><div style=""><font style="font-size: 10px;"><span style="color: rgb(246, 246, 244);">nav_2d_utils</span><span style="color: rgb(242, 134, 196);">::</span><span style="color: rgb(98, 232, 132);">twist2Dto3D</span></font></div></div>" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" vertex="1" connectable="0" parent="A1EfnVpFrXilNoYTl5xU-55">
|
||||
<mxGeometry x="-0.174" y="2" relative="1" as="geometry">
|
||||
<mxPoint x="-12" y="-13" as="offset" />
|
||||
</mxGeometry>
|
||||
</mxCell>
|
||||
</root>
|
||||
</mxGraphModel>
|
||||
</diagram>
|
||||
</mxfile>
|
||||
80
navigations/dwb_local_planner/CMakeLists.txt
Executable file
80
navigations/dwb_local_planner/CMakeLists.txt
Executable file
@@ -0,0 +1,80 @@
|
||||
cmake_minimum_required(VERSION 3.0.2)
|
||||
project(dwb_local_planner)
|
||||
|
||||
set_directory_properties(PROPERTIES COMPILE_OPTIONS "-std=c++11;-Wall;-Werror")
|
||||
|
||||
find_package(catkin REQUIRED COMPONENTS
|
||||
dwb_msgs
|
||||
geometry_msgs
|
||||
nav_2d_msgs
|
||||
nav_2d_utils
|
||||
nav_core2
|
||||
nav_msgs
|
||||
pluginlib
|
||||
roscpp
|
||||
sensor_msgs
|
||||
tf
|
||||
visualization_msgs
|
||||
)
|
||||
|
||||
catkin_package(
|
||||
INCLUDE_DIRS include
|
||||
LIBRARIES dwb_local_planner debug_dwb_local_planner trajectory_utils
|
||||
CATKIN_DEPENDS
|
||||
dwb_msgs geometry_msgs nav_2d_msgs nav_2d_utils nav_core2
|
||||
nav_msgs pluginlib roscpp sensor_msgs tf visualization_msgs
|
||||
)
|
||||
|
||||
include_directories(
|
||||
include
|
||||
${catkin_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
add_library(dwb_local_planner src/dwb_local_planner.cpp
|
||||
src/backwards_compatibility.cpp
|
||||
src/publisher.cpp
|
||||
src/illegal_trajectory_tracker.cpp
|
||||
)
|
||||
target_link_libraries(dwb_local_planner ${catkin_LIBRARIES})
|
||||
add_dependencies(dwb_local_planner ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
add_library(debug_dwb_local_planner src/debug_dwb_local_planner.cpp)
|
||||
target_link_libraries(debug_dwb_local_planner ${catkin_LIBRARIES} dwb_local_planner)
|
||||
add_dependencies(debug_dwb_local_planner ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
add_library(trajectory_utils src/trajectory_utils.cpp)
|
||||
target_link_libraries(trajectory_utils ${catkin_LIBRARIES})
|
||||
|
||||
add_executable(${PROJECT_NAME}_planner_node src/planner_node.cpp)
|
||||
target_link_libraries(${PROJECT_NAME}_planner_node ${catkin_LIBRARIES} debug_dwb_local_planner)
|
||||
add_dependencies(${PROJECT_NAME}_planner_node ${catkin_EXPORTED_TARGETS})
|
||||
set_target_properties(${PROJECT_NAME}_planner_node PROPERTIES OUTPUT_NAME planner_node PREFIX "")
|
||||
|
||||
if(CATKIN_ENABLE_TESTING)
|
||||
find_package(rostest REQUIRED)
|
||||
find_package(roslint REQUIRED)
|
||||
roslint_cpp()
|
||||
roslint_add_test()
|
||||
|
||||
catkin_add_gtest(utils_test test/utils_test.cpp)
|
||||
target_link_libraries(utils_test trajectory_utils)
|
||||
endif()
|
||||
|
||||
install(TARGETS ${PROJECT_NAME}_planner_node
|
||||
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
)
|
||||
install(TARGETS dwb_local_planner debug_dwb_local_planner trajectory_utils
|
||||
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
|
||||
)
|
||||
install(DIRECTORY include/${PROJECT_NAME}/
|
||||
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
|
||||
)
|
||||
install(FILES plugins.xml
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
||||
)
|
||||
install(DIRECTORY launch/
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
|
||||
FILES_MATCHING PATTERN "*.launch"
|
||||
)
|
||||
100
navigations/dwb_local_planner/README.md
Executable file
100
navigations/dwb_local_planner/README.md
Executable file
@@ -0,0 +1,100 @@
|
||||
# dwb_local_planner
|
||||
|
||||
> I suppose it is tempting, if the only tool you have is a hammer, to treat everything as if it were a nail.
|
||||
- Abraham Maslow
|
||||
|
||||
> I love/hate pluginlib.
|
||||
- David Lu!!
|
||||
|
||||
This local planner implementation rewrites and extends the functionality of `dwa_local_planner`, thus it is logically called `dwb_local_planner`. The goal is to make as many parts of the functionality as possible customizable, either through pluginlib or directly extending the implementing classes.
|
||||
|
||||
The goal of a local planner is to take the global plan and local costmap and produce the command velocities that will presumably move the robot to the goal. Both dwa and dwb do this via sampling, i.e. generating plausible velocity commands and evaluating them on various metrics and selecting the one with the best score, until the robot reaches its goal.
|
||||
|
||||
The form of the evaluation is critical. Let's say we are evaluating a given command to see if it collides with any obstacles in the costmap. The key question is where the robot will drive using the command. For this, you need to know the position and velocity of the robot, plus you will need to consider the kinematics of the robot. To this end, we do not evaluate the velocities in isolation, but instead score the trajectory which contains not only the velocity, but an array of some number of sample poses that we anticipate the robot to drive along.
|
||||
|
||||
## Data Structures
|
||||
The navigation stack is only capable of navigation in 2.5 dimensions (i.e. x, y and theta), thus most of the interfaces deal with `geometry_msgs/Pose2D` and `nav_2d_msgs/Twist2D` rather than the more general `Pose` and `Twist`.
|
||||
|
||||
## Code Structure
|
||||
### Twist Generator
|
||||
The first component is the twist generator, which is responsible for determining which velocity commands are evaluating and creating trajectories from them. The interface for generating the commands is iterator-based.
|
||||
```
|
||||
void startNewIteration(const nav_2d_msgs::Twist2D& current_velocity)
|
||||
bool hasMoreTwists()
|
||||
bool nextTwist(nav_2d_msgs::Twist2D& twist)
|
||||
```
|
||||
For debugging purposes, there is also a ROS service interface for the twist generator.
|
||||
```
|
||||
# dwb_msgs/srv/GenerateTwists.srv
|
||||
nav_2d_msgs/Twist2D current_vel
|
||||
---
|
||||
nav_2d_msgs/Twist2D[] twists
|
||||
```
|
||||
|
||||
The second half is for generating the trajectory, which creates a Trajectory2D.
|
||||
```
|
||||
# dwb_msgs/msg/Trajectory2D.msg
|
||||
nav_2d_msgs/Twist2D velocity
|
||||
duration duration
|
||||
geometry_msgs/Pose2D[] poses
|
||||
```
|
||||
|
||||
Code API:
|
||||
```
|
||||
bool generateTrajectory(const geometry_msgs::Pose2D& start_pose, const nav_2d_msgs::Twist2D& start_vel,
|
||||
const nav_2d_msgs::Twist2D& cmd_vel,
|
||||
dwb_msgs::Trajectory2D& traj)
|
||||
```
|
||||
|
||||
This also has a ROS service interface.
|
||||
```
|
||||
# dwb_msgs/srv/GenerateTrajectory.srv
|
||||
geometry_msgs/Pose2D start_pose
|
||||
nav_2d_msgs/Twist2D start_vel
|
||||
nav_2d_msgs/Twist2D cmd_vel
|
||||
---
|
||||
dwb_msgs/Trajectory2D traj
|
||||
```
|
||||
|
||||
How precisely DWB performs these tasks is relegated to a plugin.
|
||||
|
||||
## Goal Checker
|
||||
Are we there yet? Another plugin determines whether the robot has reached its goal. This allows for variation in combining how accurate the xy position has to be with the rotation, and whether the robot has stopped completely.
|
||||
|
||||
Code API:
|
||||
```
|
||||
virtual bool isGoalReached(const geometry_msgs::Pose2D& query_pose, const geometry_msgs::Pose2D& goal_pose,
|
||||
const nav_2d_msgs::Twist2D& velocity)
|
||||
```
|
||||
|
||||
## Trajectory Critics
|
||||
[Critics](https://www.youtube.com/watch?v=X6I_dKUYyI4) like to give things scores. Once we know we're not a the goal and have a bunch of candidate trajectories, we evaluate them based on a collections of TrajectoryCritics. Here's the life-cycle.
|
||||
|
||||
* `void initialize(const ros::NodeHandle& planner_nh, std::string name, nav_core2::Costmap::Ptr costmap)` - called once on startup, and then calls `onInit`
|
||||
* `void onInit()` - May be overwritten to load parameters as needed.
|
||||
* `void reset()` - called at the beginning of every new navigation, i.e. when we get a new global goal via `setGoalPose`.
|
||||
* `bool prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& vel, const geometry_msgs::Pose2D& goal, const nav_2d_msgs::Path2D& global_plan)` - called once per iteration of the planner, prior to the evaluation of all the trajectories
|
||||
* `double scoreTrajectory(const dwb_msgs::Trajectory2D& traj)` - called once per trajectory
|
||||
* `void debrief(const nav_2d_msgs::Twist2D& cmd_vel)` - called after all the trajectories to notify what trajectory was chosen.
|
||||
|
||||
Each critic will provide a `double` score and has an associated scale. The score used for the trajectory as a whole will be the sum of all the critic scores multiplied by their respective scales.
|
||||
|
||||
There is also a ROS service interface associated with the scoring trajectories.
|
||||
```
|
||||
# dwb_msgs/srv/DebugLocalPlan.srv
|
||||
nav_2d_msgs/Pose2DStamped pose
|
||||
nav_2d_msgs/Twist2D velocity
|
||||
nav_2d_msgs/Path2D global_plan
|
||||
---
|
||||
LocalPlanEvaluation results
|
||||
Header header
|
||||
dwb_msgs/TrajectoryScore[] twists
|
||||
nav_2d_msgs/SampedTwist2D traj
|
||||
dwb_msgs/CriticScore[] scores
|
||||
string name
|
||||
float32 raw_score
|
||||
float32 scale
|
||||
float32 total
|
||||
uint16 best_index
|
||||
uint16 worst_index
|
||||
```
|
||||
@@ -0,0 +1,55 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
#ifndef DWB_LOCAL_PLANNER_BACKWARDS_COMPATIBILITY_H
|
||||
#define DWB_LOCAL_PLANNER_BACKWARDS_COMPATIBILITY_H
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <string>
|
||||
|
||||
namespace dwb_local_planner
|
||||
{
|
||||
|
||||
std::string getBackwardsCompatibleDefaultGenerator(const ros::NodeHandle& nh);
|
||||
|
||||
/**
|
||||
* @brief Load parameters to emulate dwa_local_planner
|
||||
*
|
||||
* If no critic parameters are specified, this function should be called
|
||||
* to load/move parameters that will emulate the behavior of dwa_local_planner
|
||||
*
|
||||
* @param nh NodeHandle to load parameters from
|
||||
*/
|
||||
void loadBackwardsCompatibleParameters(const ros::NodeHandle& nh);
|
||||
} // namespace dwb_local_planner
|
||||
#endif // DWB_LOCAL_PLANNER_BACKWARDS_COMPATIBILITY_H
|
||||
@@ -0,0 +1,81 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#ifndef DWB_LOCAL_PLANNER_DEBUG_DWB_LOCAL_PLANNER_H
|
||||
#define DWB_LOCAL_PLANNER_DEBUG_DWB_LOCAL_PLANNER_H
|
||||
|
||||
#include <dwb_local_planner/dwb_local_planner.h>
|
||||
#include <dwb_msgs/DebugLocalPlan.h>
|
||||
#include <dwb_msgs/GenerateTwists.h>
|
||||
#include <dwb_msgs/GenerateTrajectory.h>
|
||||
#include <dwb_msgs/ScoreTrajectory.h>
|
||||
#include <dwb_msgs/GetCriticScore.h>
|
||||
#include <string>
|
||||
|
||||
namespace dwb_local_planner
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief A version of DWBLocalPlanner with ROS services for the major components.
|
||||
*
|
||||
* Advertises three services: GenerateTwists, GenerateTrajectory and DebugLocalPlan
|
||||
*/
|
||||
class DebugDWBLocalPlanner: public DWBLocalPlanner
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Override the DWB constructor to also advertise the services
|
||||
*/
|
||||
void initialize(const ros::NodeHandle& parent, const std::string& name,
|
||||
TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override;
|
||||
protected:
|
||||
bool generateTwistsService(dwb_msgs::GenerateTwists::Request &req,
|
||||
dwb_msgs::GenerateTwists::Response &res);
|
||||
bool generateTrajectoryService(dwb_msgs::GenerateTrajectory::Request &req,
|
||||
dwb_msgs::GenerateTrajectory::Response &res);
|
||||
bool scoreTrajectoryService(dwb_msgs::ScoreTrajectory::Request &req,
|
||||
dwb_msgs::ScoreTrajectory::Response &res);
|
||||
bool getCriticScoreService(dwb_msgs::GetCriticScore::Request &req,
|
||||
dwb_msgs::GetCriticScore::Response &res);
|
||||
bool debugLocalPlanService(dwb_msgs::DebugLocalPlan::Request &req,
|
||||
dwb_msgs::DebugLocalPlan::Response &res);
|
||||
|
||||
TrajectoryCritic::Ptr getCritic(std::string name);
|
||||
|
||||
ros::ServiceServer twist_gen_service_, generate_traj_service_, score_service_, critic_service_, debug_service_;
|
||||
};
|
||||
|
||||
} // namespace dwb_local_planner
|
||||
|
||||
#endif // DWB_LOCAL_PLANNER_DEBUG_DWB_LOCAL_PLANNER_H
|
||||
217
navigations/dwb_local_planner/include/dwb_local_planner/dwb_local_planner.h
Executable file
217
navigations/dwb_local_planner/include/dwb_local_planner/dwb_local_planner.h
Executable file
@@ -0,0 +1,217 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#ifndef DWB_LOCAL_PLANNER_DWB_LOCAL_PLANNER_H
|
||||
#define DWB_LOCAL_PLANNER_DWB_LOCAL_PLANNER_H
|
||||
|
||||
#include <dwb_local_planner/trajectory_generator.h>
|
||||
#include <dwb_local_planner/goal_checker.h>
|
||||
#include <dwb_local_planner/trajectory_critic.h>
|
||||
#include <dwb_local_planner/publisher.h>
|
||||
#include <nav_core2/local_planner.h>
|
||||
#include <pluginlib/class_loader.h>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
namespace dwb_local_planner
|
||||
{
|
||||
|
||||
/**
|
||||
* @class DWBLocalPlanner
|
||||
* @brief Plugin-based flexible local_planner
|
||||
*/
|
||||
class DWBLocalPlanner: public nav_core2::LocalPlanner
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Constructor that brings up pluginlib loaders
|
||||
*/
|
||||
DWBLocalPlanner();
|
||||
|
||||
virtual ~DWBLocalPlanner() {}
|
||||
|
||||
/**
|
||||
* @brief nav_core2 initialization
|
||||
* @param name Namespace for this planner
|
||||
* @param tf TFListener pointer
|
||||
* @param costmap_ros Costmap pointer
|
||||
*/
|
||||
void initialize(const ros::NodeHandle& parent, const std::string& name,
|
||||
TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override;
|
||||
|
||||
/**
|
||||
* @brief nav_core2 setGoalPose - Sets the global goal pose
|
||||
* @param goal_pose The Goal Pose
|
||||
*/
|
||||
void setGoalPose(const nav_2d_msgs::Pose2DStamped& goal_pose) override;
|
||||
|
||||
/**
|
||||
* @brief nav_core2 setPlan - Sets the global plan
|
||||
* @param path The global plan
|
||||
*/
|
||||
void setPlan(const nav_2d_msgs::Path2D& path) override;
|
||||
|
||||
/**
|
||||
* @brief nav_core2 computeVelocityCommands - calculates the best command given the current pose and velocity
|
||||
*
|
||||
* It is presumed that the global plan is already set.
|
||||
*
|
||||
* This is mostly a wrapper for the protected computeVelocityCommands
|
||||
* function which has additional debugging info.
|
||||
*
|
||||
* @param pose Current robot pose
|
||||
* @param velocity Current robot velocity
|
||||
* @return The best command for the robot to drive
|
||||
*/
|
||||
nav_2d_msgs::Twist2DStamped computeVelocityCommands(const nav_2d_msgs::Pose2DStamped& pose,
|
||||
const nav_2d_msgs::Twist2D& velocity) override;
|
||||
|
||||
/**
|
||||
* @brief nav_core2 isGoalReached - Check whether the robot has reached its goal, given the current pose & velocity.
|
||||
*
|
||||
* The pose that it checks against is the last pose in the current global plan.
|
||||
* The calculation is delegated to the goal_checker plugin.
|
||||
*
|
||||
* @param pose Current pose
|
||||
* @param velocity Current velocity
|
||||
* @return True if the robot should be considered as having reached the goal.
|
||||
*/
|
||||
bool isGoalReached(const nav_2d_msgs::Pose2DStamped& pose, const nav_2d_msgs::Twist2D& velocity) override;
|
||||
|
||||
/**
|
||||
* @brief Score a given command. Can be used for testing.
|
||||
*
|
||||
* Given a trajectory, calculate the score where lower scores are better.
|
||||
* If the given (positive) score exceeds the best_score, calculation may be cut short, as the
|
||||
* score can only go up from there.
|
||||
*
|
||||
* @param traj Trajectory to check
|
||||
* @param best_score If positive, the threshold for early termination
|
||||
* @return The full scoring of the input trajectory
|
||||
*/
|
||||
virtual dwb_msgs::TrajectoryScore scoreTrajectory(const dwb_msgs::Trajectory2D& traj, double best_score = -1);
|
||||
|
||||
/**
|
||||
* @brief Compute the best command given the current pose and velocity, with possible debug information
|
||||
*
|
||||
* Same as above computeVelocityCommands, but with debug results.
|
||||
* If the results pointer is not null, additional information about the twists
|
||||
* evaluated will be in results after the call.
|
||||
*
|
||||
* @param pose Current robot pose
|
||||
* @param velocity Current robot velocity
|
||||
* @param results Output param, if not null, will be filled in with full evaluation results
|
||||
* @return Best command
|
||||
*/
|
||||
virtual nav_2d_msgs::Twist2DStamped computeVelocityCommands(const nav_2d_msgs::Pose2DStamped& pose,
|
||||
const nav_2d_msgs::Twist2D& velocity,
|
||||
std::shared_ptr<dwb_msgs::LocalPlanEvaluation>& results);
|
||||
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief Helper method for preparing for the core scoring algorithm
|
||||
*
|
||||
* Runs the prepare method on all the critics with freshly transformed data
|
||||
*/
|
||||
virtual void prepare(const nav_2d_msgs::Pose2DStamped& pose, const nav_2d_msgs::Twist2D& velocity);
|
||||
|
||||
/**
|
||||
* @brief Iterate through all the twists and find the best one
|
||||
*/
|
||||
virtual dwb_msgs::TrajectoryScore coreScoringAlgorithm(const geometry_msgs::Pose2D& pose,
|
||||
const nav_2d_msgs::Twist2D velocity,
|
||||
std::shared_ptr<dwb_msgs::LocalPlanEvaluation>& results);
|
||||
|
||||
/**
|
||||
* @brief Transforms global plan into same frame as pose, clips far away poses and possibly prunes passed poses
|
||||
*
|
||||
* Three key operations
|
||||
* 1) Transforms global plan into frame of the given pose
|
||||
* 2) Only returns poses that are near the robot, i.e. whether they are likely on the local costmap
|
||||
* 3) If prune_plan_ is true, it will remove all points that we've already passed from both the transformed plan
|
||||
* and the saved global_plan_. Technically, it iterates to a pose on the path that is within prune_distance_
|
||||
* of the robot and erases all poses before that.
|
||||
*/
|
||||
virtual nav_2d_msgs::Path2D transformGlobalPlan(const nav_2d_msgs::Pose2DStamped& pose);
|
||||
|
||||
/**
|
||||
* @brief Helper method to transform a given pose to the local costmap frame.
|
||||
*/
|
||||
geometry_msgs::Pose2D transformPoseToLocal(const nav_2d_msgs::Pose2DStamped& pose);
|
||||
|
||||
nav_2d_msgs::Path2D global_plan_; ///< Saved Global Plan
|
||||
nav_2d_msgs::Pose2DStamped goal_pose_; ///< Saved Goal Pose
|
||||
bool prune_plan_;
|
||||
double prune_distance_;
|
||||
bool debug_trajectory_details_;
|
||||
bool short_circuit_trajectory_evaluation_;
|
||||
|
||||
// Plugin handling
|
||||
pluginlib::ClassLoader<TrajectoryGenerator> traj_gen_loader_;
|
||||
TrajectoryGenerator::Ptr traj_generator_;
|
||||
pluginlib::ClassLoader<GoalChecker> goal_checker_loader_;
|
||||
GoalChecker::Ptr goal_checker_;
|
||||
pluginlib::ClassLoader<TrajectoryCritic> critic_loader_;
|
||||
std::vector<TrajectoryCritic::Ptr> critics_;
|
||||
|
||||
/**
|
||||
* @brief try to resolve a possibly shortened critic name with the default namespaces and the suffix "Critic"
|
||||
*
|
||||
* @param base_name The name of the critic as read in from the parameter server
|
||||
* @return Our attempted resolution of the name, with namespace prepended and/or the suffix Critic appended
|
||||
*/
|
||||
std::string resolveCriticClassName(std::string base_name);
|
||||
|
||||
/**
|
||||
* @brief Load the critic parameters from the namespace
|
||||
* @param name The namespace of this planner.
|
||||
*/
|
||||
virtual void loadCritics(const std::string name);
|
||||
|
||||
std::vector<std::string> default_critic_namespaces_;
|
||||
|
||||
nav_core2::Costmap::Ptr costmap_;
|
||||
bool update_costmap_before_planning_;
|
||||
TFListenerPtr tf_;
|
||||
DWBPublisher pub_;
|
||||
|
||||
ros::NodeHandle planner_nh_;
|
||||
ros::Publisher traj_pub_;
|
||||
};
|
||||
|
||||
} // namespace dwb_local_planner
|
||||
|
||||
#endif // DWB_LOCAL_PLANNER_DWB_LOCAL_PLANNER_H
|
||||
86
navigations/dwb_local_planner/include/dwb_local_planner/goal_checker.h
Executable file
86
navigations/dwb_local_planner/include/dwb_local_planner/goal_checker.h
Executable file
@@ -0,0 +1,86 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#ifndef DWB_LOCAL_PLANNER_GOAL_CHECKER_H
|
||||
#define DWB_LOCAL_PLANNER_GOAL_CHECKER_H
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <geometry_msgs/Pose2D.h>
|
||||
#include <nav_2d_msgs/Twist2D.h>
|
||||
#include <memory>
|
||||
|
||||
namespace dwb_local_planner
|
||||
{
|
||||
|
||||
/**
|
||||
* @class GoalChecker
|
||||
* @brief Function-object for checking whether a goal has been reached
|
||||
*
|
||||
* This class defines the plugin interface for determining whether you have reached
|
||||
* the goal state. This primarily consists of checking the relative positions of two poses
|
||||
* (which are presumed to be in the same frame). It can also check the velocity, as some
|
||||
* applications require that robot be stopped to be considered as having reached the goal.
|
||||
*/
|
||||
class GoalChecker
|
||||
{
|
||||
public:
|
||||
using Ptr = std::shared_ptr<dwb_local_planner::GoalChecker>;
|
||||
|
||||
virtual ~GoalChecker() {}
|
||||
|
||||
/**
|
||||
* @brief Initialize any parameters from the NodeHandle
|
||||
* @param nh NodeHandle for grabbing parameters
|
||||
*/
|
||||
virtual void initialize(const ros::NodeHandle& nh) = 0;
|
||||
|
||||
/**
|
||||
* @brief Reset the state of the goal checker (if any)
|
||||
*/
|
||||
virtual void reset() {}
|
||||
|
||||
/**
|
||||
* @brief Check whether the goal should be considered reached
|
||||
* @param query_pose The pose to check
|
||||
* @param goal_pose The pose to check against
|
||||
* @param velocity The robot's current velocity
|
||||
* @return True if goal is reached
|
||||
*/
|
||||
virtual bool isGoalReached(const geometry_msgs::Pose2D& query_pose, const geometry_msgs::Pose2D& goal_pose,
|
||||
const nav_2d_msgs::Twist2D& velocity) = 0;
|
||||
};
|
||||
|
||||
} // namespace dwb_local_planner
|
||||
|
||||
#endif // DWB_LOCAL_PLANNER_GOAL_CHECKER_H
|
||||
@@ -0,0 +1,75 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#ifndef DWB_LOCAL_PLANNER_ILLEGAL_TRAJECTORY_TRACKER_H
|
||||
#define DWB_LOCAL_PLANNER_ILLEGAL_TRAJECTORY_TRACKER_H
|
||||
|
||||
#include <nav_core2/exceptions.h>
|
||||
#include <map>
|
||||
#include <utility>
|
||||
#include <string>
|
||||
|
||||
namespace dwb_local_planner
|
||||
{
|
||||
class IllegalTrajectoryTracker
|
||||
{
|
||||
public:
|
||||
IllegalTrajectoryTracker() : legal_count_(0), illegal_count_(0) {}
|
||||
|
||||
void addIllegalTrajectory(const nav_core2::IllegalTrajectoryException& e);
|
||||
void addLegalTrajectory();
|
||||
|
||||
std::map< std::pair<std::string, std::string>, double> getPercentages() const;
|
||||
|
||||
std::string getMessage() const;
|
||||
protected:
|
||||
std::map< std::pair<std::string, std::string>, unsigned int> counts_;
|
||||
unsigned int legal_count_, illegal_count_;
|
||||
};
|
||||
|
||||
/**
|
||||
* @class NoLegalTrajectoriesException
|
||||
* @brief Thrown when all the trajectories explored are illegal
|
||||
*/
|
||||
class NoLegalTrajectoriesException: public nav_core2::NoLegalTrajectoriesException
|
||||
{
|
||||
public:
|
||||
explicit NoLegalTrajectoriesException(const IllegalTrajectoryTracker& tracker)
|
||||
: nav_core2::NoLegalTrajectoriesException(tracker.getMessage()), tracker_(tracker) {}
|
||||
IllegalTrajectoryTracker tracker_;
|
||||
};
|
||||
|
||||
} // namespace dwb_local_planner
|
||||
|
||||
#endif // DWB_LOCAL_PLANNER_ILLEGAL_TRAJECTORY_TRACKER_H
|
||||
107
navigations/dwb_local_planner/include/dwb_local_planner/publisher.h
Executable file
107
navigations/dwb_local_planner/include/dwb_local_planner/publisher.h
Executable file
@@ -0,0 +1,107 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#ifndef DWB_LOCAL_PLANNER_PUBLISHER_H
|
||||
#define DWB_LOCAL_PLANNER_PUBLISHER_H
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <nav_core2/common.h>
|
||||
#include <dwb_local_planner/trajectory_critic.h>
|
||||
#include <dwb_msgs/LocalPlanEvaluation.h>
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
|
||||
namespace dwb_local_planner
|
||||
{
|
||||
|
||||
/**
|
||||
* @class DWBPublisher
|
||||
* @brief Consolidation of all the publishing logic for the DWB Local Planner.
|
||||
*
|
||||
* Right now, it can publish
|
||||
* 1) The Global Plan (as passed in using setPath)
|
||||
* 2) The Local Plan (after it is calculated)
|
||||
* 3) The Transformed Global Plan (since it may be different than the global)
|
||||
* 4) The Full LocalPlanEvaluation
|
||||
* 5) Markers representing the different trajectories evaluated
|
||||
* 6) The CostGrid (in the form of a complex PointCloud2)
|
||||
*/
|
||||
class DWBPublisher
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Load the parameters and advertise topics as needed
|
||||
* @param nh NodeHandle to load parameters from
|
||||
*/
|
||||
void initialize(ros::NodeHandle& nh);
|
||||
|
||||
/**
|
||||
* @brief Does the publisher require that the LocalPlanEvaluation be saved
|
||||
* @return True if the Evaluation is needed to publish either directly or as trajectories
|
||||
*/
|
||||
bool shouldRecordEvaluation() { return publish_evaluation_ || publish_trajectories_; }
|
||||
|
||||
/**
|
||||
* @brief If the pointer is not null, publish the evaluation and trajectories as needed
|
||||
*/
|
||||
void publishEvaluation(std::shared_ptr<dwb_msgs::LocalPlanEvaluation> results);
|
||||
void publishLocalPlan(const std_msgs::Header& header, const dwb_msgs::Trajectory2D& traj);
|
||||
void publishCostGrid(const nav_core2::Costmap::Ptr costmap, const std::vector<TrajectoryCritic::Ptr> critics);
|
||||
void publishGlobalPlan(const nav_2d_msgs::Path2D plan);
|
||||
void publishTransformedPlan(const nav_2d_msgs::Path2D plan);
|
||||
void publishLocalPlan(const nav_2d_msgs::Path2D plan);
|
||||
void publishInputParams(const nav_grid::NavGridInfo& info, const geometry_msgs::Pose2D& start_pose,
|
||||
const nav_2d_msgs::Twist2D& velocity, const geometry_msgs::Pose2D& goal_pose);
|
||||
|
||||
protected:
|
||||
void publishTrajectories(const dwb_msgs::LocalPlanEvaluation& results);
|
||||
|
||||
// Helper function for publishing other plans
|
||||
void publishGenericPlan(const nav_2d_msgs::Path2D plan, const ros::Publisher pub, bool flag);
|
||||
|
||||
// Flags for turning on/off publishing specific components
|
||||
bool publish_evaluation_, publish_global_plan_, publish_transformed_, publish_local_plan_, publish_trajectories_;
|
||||
bool publish_cost_grid_pc_, publish_input_params_;
|
||||
|
||||
// Marker Lifetime
|
||||
ros::Duration marker_lifetime_;
|
||||
|
||||
// Publisher Objects
|
||||
ros::Publisher eval_pub_, global_pub_, transformed_pub_, local_pub_, marker_pub_, cost_grid_pc_pub_,
|
||||
info_pub_, pose_pub_, goal_pub_, velocity_pub_;
|
||||
};
|
||||
|
||||
} // namespace dwb_local_planner
|
||||
|
||||
#endif // DWB_LOCAL_PLANNER_PUBLISHER_H
|
||||
177
navigations/dwb_local_planner/include/dwb_local_planner/trajectory_critic.h
Executable file
177
navigations/dwb_local_planner/include/dwb_local_planner/trajectory_critic.h
Executable file
@@ -0,0 +1,177 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#ifndef DWB_LOCAL_PLANNER_TRAJECTORY_CRITIC_H
|
||||
#define DWB_LOCAL_PLANNER_TRAJECTORY_CRITIC_H
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <nav_core2/common.h>
|
||||
#include <nav_core2/costmap.h>
|
||||
#include <geometry_msgs/Pose2D.h>
|
||||
#include <nav_2d_msgs/Twist2D.h>
|
||||
#include <nav_2d_msgs/Path2D.h>
|
||||
#include <dwb_msgs/Trajectory2D.h>
|
||||
#include <sensor_msgs/PointCloud.h>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
namespace dwb_local_planner
|
||||
{
|
||||
/**
|
||||
* @class TrajectoryCritic
|
||||
* @brief Evaluates a Trajectory2D to produce a score
|
||||
*
|
||||
* This class defines the plugin interface for the TrajectoryCritic which
|
||||
* gives scores to trajectories, where lower numbers are better, but negative
|
||||
* scores are considered invalid.
|
||||
*
|
||||
* The general lifecycle is
|
||||
* 1) initialize is called once at the beginning which in turn calls onInit.
|
||||
* Derived classes may override onInit to load parameters as needed.
|
||||
* 2) prepare is called once before each set of trajectories.
|
||||
* It is presumed that there are multiple trajectories that we want to evaluate,
|
||||
* and there may be some shared work that can be done beforehand to optimize
|
||||
* the scoring of each individual trajectory.
|
||||
* 3) scoreTrajectory is called once per trajectory and returns the score.
|
||||
* 4) debrief is called after each set of trajectories with the chosen trajectory.
|
||||
* This can be used for stateful critics that monitor the trajectory through time.
|
||||
*
|
||||
* Optionally, there is also a debugging mechanism for certain types of critics in the
|
||||
* addCriticVisualization method. If the score for a trajectory depends on its relationship to
|
||||
* the costmap, addCriticVisualization can provide that information to the dwb_local_planner
|
||||
* which will publish the grid scores as a PointCloud2.
|
||||
*/
|
||||
class TrajectoryCritic
|
||||
{
|
||||
public:
|
||||
using Ptr = std::shared_ptr<dwb_local_planner::TrajectoryCritic>;
|
||||
|
||||
virtual ~TrajectoryCritic() {}
|
||||
|
||||
/**
|
||||
* @brief Initialize the critic with appropriate pointers and parameters
|
||||
*
|
||||
* The name and costmap are stored as member variables.
|
||||
* A NodeHandle for the critic is created with the namespace of the planner NodeHandle
|
||||
*
|
||||
* @param planner_nh Planner Nodehandle
|
||||
* @param name The name of this critic
|
||||
* @param costmap_ros Pointer to the costmap
|
||||
*/
|
||||
void initialize(const ros::NodeHandle& planner_nh, std::string name, nav_core2::Costmap::Ptr costmap)
|
||||
{
|
||||
name_ = name;
|
||||
costmap_ = costmap;
|
||||
planner_nh_ = planner_nh;
|
||||
critic_nh_ = ros::NodeHandle(planner_nh_, name_);
|
||||
critic_nh_.param("scale", scale_, 1.0);
|
||||
onInit();
|
||||
}
|
||||
|
||||
virtual void onInit() {}
|
||||
|
||||
/**
|
||||
* @brief Reset the state of the critic
|
||||
*
|
||||
* Reset is called when the planner receives a new global goal.
|
||||
* This can be used to discard information specific to one plan.
|
||||
*/
|
||||
virtual void reset() {}
|
||||
|
||||
/**
|
||||
* @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories
|
||||
*
|
||||
* Subclasses may overwrite. Return false in case there is any error.
|
||||
*
|
||||
* @param pose Current pose (costmap frame)
|
||||
* @param vel Current velocity
|
||||
* @param goal The final goal (costmap frame)
|
||||
* @param global_plan Transformed global plan in costmap frame, possibly cropped to nearby points
|
||||
*/
|
||||
virtual bool prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& vel,
|
||||
const geometry_msgs::Pose2D& goal,
|
||||
const nav_2d_msgs::Path2D& global_plan)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Return a raw score for the given trajectory.
|
||||
*
|
||||
* scores < 0 are considered invalid/errors, such as collisions
|
||||
* This is the raw score in that the scale should not be applied to it.
|
||||
*/
|
||||
virtual double scoreTrajectory(const dwb_msgs::Trajectory2D& traj) = 0;
|
||||
|
||||
/**
|
||||
* @brief debrief informs the critic what the chosen cmd_vel was (if it cares)
|
||||
*/
|
||||
virtual void debrief(const nav_2d_msgs::Twist2D& cmd_vel) {}
|
||||
|
||||
/**
|
||||
* @brief Add information to the given pointcloud for debugging costmap-grid based scores
|
||||
*
|
||||
* addCriticVisualization is an optional debugging mechanism for providing rich information
|
||||
* about the cost for certain trajectories. Some critics will have scoring mechanisms
|
||||
* wherein there will be some score for each cell in the costmap. This could be as
|
||||
* straightforward as the cost in the costmap, or it could be the number of cells away
|
||||
* from the goal pose.
|
||||
*
|
||||
* Prior to calling this, dwb_local_planner will load the PointCloud's header and the points
|
||||
* in row-major order. The critic may then add a ChannelFloat to the channels member of the PC
|
||||
* with the same number of values as the points array. This information may then be converted
|
||||
* and published as a PointCloud2.
|
||||
*
|
||||
* @param pc PointCloud to add channels to
|
||||
*/
|
||||
virtual void addCriticVisualization(sensor_msgs::PointCloud& pc) {}
|
||||
|
||||
std::string getName()
|
||||
{
|
||||
return name_;
|
||||
}
|
||||
|
||||
virtual double getScale() const { return scale_; }
|
||||
void setScale(const double scale) { scale_ = scale; }
|
||||
protected:
|
||||
std::string name_;
|
||||
nav_core2::Costmap::Ptr costmap_;
|
||||
double scale_;
|
||||
ros::NodeHandle critic_nh_, planner_nh_;
|
||||
};
|
||||
|
||||
} // namespace dwb_local_planner
|
||||
|
||||
#endif // DWB_LOCAL_PLANNER_TRAJECTORY_CRITIC_H
|
||||
130
navigations/dwb_local_planner/include/dwb_local_planner/trajectory_generator.h
Executable file
130
navigations/dwb_local_planner/include/dwb_local_planner/trajectory_generator.h
Executable file
@@ -0,0 +1,130 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#ifndef DWB_LOCAL_PLANNER_TRAJECTORY_GENERATOR_H
|
||||
#define DWB_LOCAL_PLANNER_TRAJECTORY_GENERATOR_H
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <nav_2d_msgs/Twist2D.h>
|
||||
#include <dwb_msgs/Trajectory2D.h>
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
|
||||
namespace dwb_local_planner
|
||||
{
|
||||
|
||||
/**
|
||||
* @class TrajectoryGenerator
|
||||
* @brief Interface for iterating through possible velocities and creating trajectories
|
||||
*
|
||||
* This class defines the plugin interface for two separate but related components.
|
||||
*
|
||||
* First, this class provides an iterator interface for exploring all of the velocities
|
||||
* to search, given the current velocity.
|
||||
*
|
||||
* Second, the class gives an independent interface for creating a trajectory from a twist,
|
||||
* i.e. projecting it out in time and space.
|
||||
*
|
||||
* Both components rely heavily on the robot's kinematic model, and can share many parameters,
|
||||
* which is why they are grouped into a singular class.
|
||||
*/
|
||||
class TrajectoryGenerator
|
||||
{
|
||||
public:
|
||||
using Ptr = std::shared_ptr<dwb_local_planner::TrajectoryGenerator>;
|
||||
|
||||
virtual ~TrajectoryGenerator() {}
|
||||
|
||||
/**
|
||||
* @brief Initialize parameters as needed
|
||||
* @param nh NodeHandle to read parameters from
|
||||
*/
|
||||
virtual void initialize(ros::NodeHandle& nh) = 0;
|
||||
|
||||
/**
|
||||
* @brief Reset the state (if any) when the planner gets a new goal
|
||||
*/
|
||||
virtual void reset() {}
|
||||
|
||||
/**
|
||||
* @brief Start a new iteration based on the current velocity
|
||||
* @param current_velocity
|
||||
*/
|
||||
virtual void startNewIteration(const nav_2d_msgs::Twist2D& current_velocity) = 0;
|
||||
|
||||
/**
|
||||
* @brief Test to see whether there are more twists to test
|
||||
* @return True if more twists, false otherwise
|
||||
*/
|
||||
virtual bool hasMoreTwists() = 0;
|
||||
|
||||
/**
|
||||
* @brief Return the next twist and advance the iteration
|
||||
* @return The Twist!
|
||||
*/
|
||||
virtual nav_2d_msgs::Twist2D nextTwist() = 0;
|
||||
|
||||
/**
|
||||
* @brief Get all the twists for an iteration.
|
||||
*
|
||||
* Note: Resets the iterator if one is in process
|
||||
*
|
||||
* @param current_velocity
|
||||
* @return all the twists
|
||||
*/
|
||||
virtual std::vector<nav_2d_msgs::Twist2D> getTwists(const nav_2d_msgs::Twist2D& current_velocity)
|
||||
{
|
||||
std::vector<nav_2d_msgs::Twist2D> twists;
|
||||
startNewIteration(current_velocity);
|
||||
while (hasMoreTwists())
|
||||
{
|
||||
twists.push_back(nextTwist());
|
||||
}
|
||||
return twists;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Given a cmd_vel in the robot's frame and initial conditions, generate a Trajectory2D
|
||||
* @param start_pose Current robot location
|
||||
* @param start_vel Current robot velocity
|
||||
* @param cmd_vel The desired command velocity
|
||||
*/
|
||||
virtual dwb_msgs::Trajectory2D generateTrajectory(const geometry_msgs::Pose2D& start_pose,
|
||||
const nav_2d_msgs::Twist2D& start_vel,
|
||||
const nav_2d_msgs::Twist2D& cmd_vel) = 0;
|
||||
};
|
||||
|
||||
} // namespace dwb_local_planner
|
||||
|
||||
#endif // DWB_LOCAL_PLANNER_TRAJECTORY_GENERATOR_H
|
||||
65
navigations/dwb_local_planner/include/dwb_local_planner/trajectory_utils.h
Executable file
65
navigations/dwb_local_planner/include/dwb_local_planner/trajectory_utils.h
Executable file
@@ -0,0 +1,65 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2018, Locus Robotics
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#ifndef DWB_LOCAL_PLANNER_TRAJECTORY_UTILS_H
|
||||
#define DWB_LOCAL_PLANNER_TRAJECTORY_UTILS_H
|
||||
|
||||
#include <dwb_msgs/Trajectory2D.h>
|
||||
|
||||
namespace dwb_local_planner
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief Helper function to find a pose in the trajectory with a particular time time_offset
|
||||
* @param trajectory The trajectory to search
|
||||
* @param time_offset The desired time_offset
|
||||
* @return reference to the pose that is closest to the particular time offset
|
||||
*
|
||||
* Linearly searches through the poses. Once the poses time_offset is greater than the desired time_offset,
|
||||
* the search ends, since the poses have increasing time_offsets.
|
||||
*/
|
||||
const geometry_msgs::Pose2D& getClosestPose(const dwb_msgs::Trajectory2D& trajectory, const double time_offset);
|
||||
|
||||
/**
|
||||
* @brief Helper function to create a pose with an exact time_offset by linearly interpolating between existing poses
|
||||
* @param trajectory The trajectory with pose and time offset information
|
||||
* @param time_offset The desired time_offset
|
||||
* @return New Pose2D with interpolated values
|
||||
* @note If the given time offset is outside the bounds of the trajectory, the return pose will be either the first or last pose.
|
||||
*/
|
||||
geometry_msgs::Pose2D projectPose(const dwb_msgs::Trajectory2D& trajectory, const double time_offset);
|
||||
|
||||
} // namespace dwb_local_planner
|
||||
|
||||
#endif // DWB_LOCAL_PLANNER_TRAJECTORY_UTILS_H
|
||||
14
navigations/dwb_local_planner/launch/plan_node.launch
Executable file
14
navigations/dwb_local_planner/launch/plan_node.launch
Executable file
@@ -0,0 +1,14 @@
|
||||
<launch>
|
||||
<node name="plan_node" pkg="dwb_local_planner" type="planner_node" output="screen">
|
||||
<rosparam ns="costmap">
|
||||
wait_for_transform: false
|
||||
pose_update_frequency: -1.0
|
||||
update_frequency: -1.0
|
||||
origin_x: -5.0
|
||||
origin_y: -5.0
|
||||
global_frame: /odom
|
||||
</rosparam>
|
||||
<rosparam ns="dwb_local_planner">
|
||||
</rosparam>
|
||||
</node>
|
||||
</launch>
|
||||
32
navigations/dwb_local_planner/package.xml
Executable file
32
navigations/dwb_local_planner/package.xml
Executable file
@@ -0,0 +1,32 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>dwb_local_planner</name>
|
||||
<version>0.3.0</version>
|
||||
<description>
|
||||
Plugin based local planner implementing the nav_core2::LocalPlanner interface.
|
||||
</description>
|
||||
|
||||
<maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
|
||||
<author email="davidvlu@gmail.com">David V. Lu!!</author>
|
||||
|
||||
<license>BSD</license>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<depend>dwb_msgs</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>nav_2d_msgs</depend>
|
||||
<depend>nav_2d_utils</depend>
|
||||
<depend>nav_core2</depend>
|
||||
<depend>nav_msgs</depend>
|
||||
<depend>pluginlib</depend>
|
||||
<depend>roscpp</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>tf</depend>
|
||||
<depend>visualization_msgs</depend>
|
||||
<test_depend>roslint</test_depend>
|
||||
<test_depend>rostest</test_depend>
|
||||
<test_depend>rosunit</test_depend>
|
||||
<export>
|
||||
<nav_core2 plugin="${prefix}/plugins.xml"/>
|
||||
</export>
|
||||
</package>
|
||||
12
navigations/dwb_local_planner/plugins.xml
Executable file
12
navigations/dwb_local_planner/plugins.xml
Executable file
@@ -0,0 +1,12 @@
|
||||
<class_libraries>
|
||||
<library path="lib/libdwb_local_planner">
|
||||
<class type="dwb_local_planner::DWBLocalPlanner" base_class_type="nav_core2::LocalPlanner">
|
||||
<description></description>
|
||||
</class>
|
||||
</library>
|
||||
<library path="lib/libdebug_dwb_local_planner">
|
||||
<class type="dwb_local_planner::DebugDWBLocalPlanner" base_class_type="nav_core2::LocalPlanner">
|
||||
<description></description>
|
||||
</class>
|
||||
</library>
|
||||
</class_libraries>
|
||||
81
navigations/dwb_local_planner/src/backwards_compatibility.cpp
Executable file
81
navigations/dwb_local_planner/src/backwards_compatibility.cpp
Executable file
@@ -0,0 +1,81 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
#include <dwb_local_planner/backwards_compatibility.h>
|
||||
#include <nav_2d_utils/parameters.h>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
namespace dwb_local_planner
|
||||
{
|
||||
|
||||
using nav_2d_utils::moveParameter;
|
||||
|
||||
std::string getBackwardsCompatibleDefaultGenerator(const ros::NodeHandle& nh)
|
||||
{
|
||||
bool use_dwa;
|
||||
nh.param("use_dwa", use_dwa, true);
|
||||
if (use_dwa)
|
||||
{
|
||||
return "dwb_plugins::LimitedAccelGenerator";
|
||||
}
|
||||
else
|
||||
{
|
||||
return "dwb_plugins::StandardTrajectoryGenerator";
|
||||
}
|
||||
}
|
||||
|
||||
void loadBackwardsCompatibleParameters(const ros::NodeHandle& nh)
|
||||
{
|
||||
std::vector<std::string> critic_names;
|
||||
ROS_INFO_NAMED("DWBLocalPlanner", "No critics configured! Using the default set.");
|
||||
critic_names.push_back("RotateToGoal"); // discards trajectories that move forward when already at goal
|
||||
critic_names.push_back("Oscillation"); // discards oscillating motions (assisgns cost -1)
|
||||
critic_names.push_back("ObstacleFootprint"); // discards trajectories that move into obstacles
|
||||
critic_names.push_back("GoalAlign"); // prefers trajectories that make the nose go towards (local) nose goal
|
||||
critic_names.push_back("PathAlign"); // prefers trajectories that keep the robot nose on nose path
|
||||
critic_names.push_back("PathDist"); // prefers trajectories on global path
|
||||
critic_names.push_back("GoalDist"); // prefers trajectories that go towards (local) goal,
|
||||
// based on wave propagation
|
||||
nh.setParam("critics", critic_names);
|
||||
moveParameter(nh, "path_distance_bias", "PathAlign/scale", 32.0, false);
|
||||
moveParameter(nh, "goal_distance_bias", "GoalAlign/scale", 24.0, false);
|
||||
moveParameter(nh, "path_distance_bias", "PathDist/scale", 32.0);
|
||||
moveParameter(nh, "goal_distance_bias", "GoalDist/scale", 24.0);
|
||||
moveParameter(nh, "occdist_scale", "ObstacleFootprint/scale", 0.01);
|
||||
|
||||
moveParameter(nh, "max_scaling_factor", "ObstacleFootprint/max_scaling_factor", 0.2);
|
||||
moveParameter(nh, "scaling_speed", "ObstacleFootprint/scaling_speed", 0.25);
|
||||
}
|
||||
|
||||
} // namespace dwb_local_planner
|
||||
150
navigations/dwb_local_planner/src/debug_dwb_local_planner.cpp
Executable file
150
navigations/dwb_local_planner/src/debug_dwb_local_planner.cpp
Executable file
@@ -0,0 +1,150 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include <dwb_local_planner/debug_dwb_local_planner.h>
|
||||
#include <nav_2d_utils/tf_help.h>
|
||||
#include <nav_core2/exceptions.h>
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
namespace dwb_local_planner
|
||||
{
|
||||
|
||||
void DebugDWBLocalPlanner::initialize(const ros::NodeHandle& parent, const std::string& name,
|
||||
TFListenerPtr tf, nav_core2::Costmap::Ptr costmap)
|
||||
{
|
||||
DWBLocalPlanner::initialize(parent, name, tf, costmap);
|
||||
|
||||
debug_service_ = planner_nh_.advertiseService("debug_local_plan",
|
||||
&DebugDWBLocalPlanner::debugLocalPlanService, this);
|
||||
twist_gen_service_ = planner_nh_.advertiseService("generate_twists",
|
||||
&DebugDWBLocalPlanner::generateTwistsService, this);
|
||||
score_service_ = planner_nh_.advertiseService("score_trajectory",
|
||||
&DebugDWBLocalPlanner::scoreTrajectoryService, this);
|
||||
critic_service_ = planner_nh_.advertiseService("get_critic_score",
|
||||
&DebugDWBLocalPlanner::getCriticScoreService, this);
|
||||
generate_traj_service_ = planner_nh_.advertiseService("generate_traj",
|
||||
&DebugDWBLocalPlanner::generateTrajectoryService, this);
|
||||
}
|
||||
|
||||
bool DebugDWBLocalPlanner::generateTwistsService(dwb_msgs::GenerateTwists::Request &req,
|
||||
dwb_msgs::GenerateTwists::Response &res)
|
||||
{
|
||||
res.twists = traj_generator_->getTwists(req.current_vel);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool DebugDWBLocalPlanner::generateTrajectoryService(dwb_msgs::GenerateTrajectory::Request &req,
|
||||
dwb_msgs::GenerateTrajectory::Response &res)
|
||||
{
|
||||
res.traj = traj_generator_->generateTrajectory(req.start_pose, req.start_vel, req.cmd_vel);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool DebugDWBLocalPlanner::scoreTrajectoryService(dwb_msgs::ScoreTrajectory::Request &req,
|
||||
dwb_msgs::ScoreTrajectory::Response &res)
|
||||
{
|
||||
if (req.goal.header.frame_id != "")
|
||||
setGoalPose(req.goal);
|
||||
if (req.global_plan.poses.size() > 0)
|
||||
setPlan(req.global_plan);
|
||||
|
||||
prepare(req.pose, req.velocity);
|
||||
res.score = scoreTrajectory(req.traj);
|
||||
return true;
|
||||
}
|
||||
|
||||
TrajectoryCritic::Ptr DebugDWBLocalPlanner::getCritic(std::string name)
|
||||
{
|
||||
for (TrajectoryCritic::Ptr critic : critics_)
|
||||
{
|
||||
if (critic->getName() == name)
|
||||
return critic;
|
||||
}
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
bool DebugDWBLocalPlanner::getCriticScoreService(dwb_msgs::GetCriticScore::Request &req,
|
||||
dwb_msgs::GetCriticScore::Response &res)
|
||||
{
|
||||
TrajectoryCritic::Ptr critic = getCritic(req.critic_name);
|
||||
if (critic == nullptr)
|
||||
{
|
||||
ROS_WARN_NAMED("DebugDWBLocalPlanner", "Critic %s not found!", req.critic_name.c_str());
|
||||
return false;
|
||||
}
|
||||
if (req.goal.header.frame_id != "")
|
||||
setGoalPose(req.goal);
|
||||
if (req.global_plan.poses.size() > 0)
|
||||
setPlan(req.global_plan);
|
||||
|
||||
// This prepares all the critics, even though we only need to prepare the one
|
||||
prepare(req.pose, req.velocity);
|
||||
|
||||
res.score.raw_score = critic->scoreTrajectory(req.traj);
|
||||
res.score.scale = critic->getScale();
|
||||
res.score.name = req.critic_name;
|
||||
|
||||
pub_.publishCostGrid(costmap_, critics_);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool DebugDWBLocalPlanner::debugLocalPlanService(dwb_msgs::DebugLocalPlan::Request &req,
|
||||
dwb_msgs::DebugLocalPlan::Response &res)
|
||||
{
|
||||
if (req.goal.header.frame_id != "")
|
||||
setGoalPose(req.goal);
|
||||
if (req.global_plan.poses.size() > 0)
|
||||
setPlan(req.global_plan);
|
||||
std::shared_ptr<dwb_msgs::LocalPlanEvaluation> results = std::make_shared<dwb_msgs::LocalPlanEvaluation>();
|
||||
try
|
||||
{
|
||||
computeVelocityCommands(req.pose, req.velocity, results);
|
||||
res.results = *results;
|
||||
return true;
|
||||
}
|
||||
catch (const nav_core2::PlannerException& e)
|
||||
{
|
||||
ROS_ERROR_NAMED("DebugDWBLocalPlanner", "Exception in computeVelocityCommands: %s", e.what());
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace dwb_local_planner
|
||||
|
||||
|
||||
// register this planner as a LocalPlanner plugin
|
||||
PLUGINLIB_EXPORT_CLASS(dwb_local_planner::DebugDWBLocalPlanner, nav_core2::LocalPlanner)
|
||||
506
navigations/dwb_local_planner/src/dwb_local_planner.cpp
Executable file
506
navigations/dwb_local_planner/src/dwb_local_planner.cpp
Executable file
@@ -0,0 +1,506 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include <nav_core2/exceptions.h>
|
||||
#include <dwb_local_planner/dwb_local_planner.h>
|
||||
#include <dwb_local_planner/backwards_compatibility.h>
|
||||
#include <dwb_local_planner/illegal_trajectory_tracker.h>
|
||||
#include <nav_2d_utils/conversions.h>
|
||||
#include <nav_2d_utils/tf_help.h>
|
||||
#include <nav_2d_msgs/Twist2D.h>
|
||||
#include <dwb_msgs/CriticScore.h>
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
#include <algorithm>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
namespace dwb_local_planner
|
||||
{
|
||||
|
||||
DWBLocalPlanner::DWBLocalPlanner() : traj_gen_loader_("dwb_local_planner", "dwb_local_planner::TrajectoryGenerator"),
|
||||
goal_checker_loader_("dwb_local_planner", "dwb_local_planner::GoalChecker"),
|
||||
critic_loader_("dwb_local_planner", "dwb_local_planner::TrajectoryCritic")
|
||||
{
|
||||
}
|
||||
|
||||
void DWBLocalPlanner::initialize(const ros::NodeHandle &parent, const std::string &name,
|
||||
TFListenerPtr tf, nav_core2::Costmap::Ptr costmap)
|
||||
{
|
||||
tf_ = tf;
|
||||
costmap_ = costmap;
|
||||
planner_nh_ = ros::NodeHandle(parent, name);
|
||||
|
||||
// This is needed when using the CostmapAdapter to ensure that the costmap's info matches the rolling window
|
||||
planner_nh_.param("update_costmap_before_planning", update_costmap_before_planning_, true);
|
||||
|
||||
planner_nh_.param("prune_plan", prune_plan_, true);
|
||||
planner_nh_.param("prune_distance", prune_distance_, 1.0);
|
||||
planner_nh_.param("short_circuit_trajectory_evaluation", short_circuit_trajectory_evaluation_, true);
|
||||
planner_nh_.param("debug_trajectory_details", debug_trajectory_details_, false);
|
||||
pub_.initialize(planner_nh_);
|
||||
traj_pub_ = planner_nh_.advertise<nav_msgs::Path>("traj_base", 1);
|
||||
// Plugins
|
||||
std::string traj_generator_name;
|
||||
planner_nh_.param("trajectory_generator_name", traj_generator_name,
|
||||
getBackwardsCompatibleDefaultGenerator(planner_nh_));
|
||||
ROS_INFO_NAMED("DWBLocalPlanner", "Using Trajectory Generator \"%s\"", traj_generator_name.c_str());
|
||||
traj_generator_ = std::move(traj_gen_loader_.createUniqueInstance(traj_generator_name));
|
||||
traj_generator_->initialize(planner_nh_);
|
||||
|
||||
std::string goal_checker_name;
|
||||
planner_nh_.param("goal_checker_name", goal_checker_name, std::string("dwb_plugins::SimpleGoalChecker"));
|
||||
ROS_INFO_NAMED("DWBLocalPlanner", "Using Goal Checker \"%s\"", goal_checker_name.c_str());
|
||||
goal_checker_ = std::move(goal_checker_loader_.createUniqueInstance(goal_checker_name));
|
||||
goal_checker_->initialize(planner_nh_);
|
||||
|
||||
loadCritics(name);
|
||||
}
|
||||
|
||||
std::string DWBLocalPlanner::resolveCriticClassName(std::string base_name)
|
||||
{
|
||||
if (base_name.find("Critic") == std::string::npos)
|
||||
{
|
||||
base_name = base_name + "Critic";
|
||||
}
|
||||
|
||||
if (base_name.find("::") == std::string::npos)
|
||||
{
|
||||
for (unsigned int j = 0; j < default_critic_namespaces_.size(); j++)
|
||||
{
|
||||
std::string full_name = default_critic_namespaces_[j] + "::" + base_name;
|
||||
if (critic_loader_.isClassAvailable(full_name))
|
||||
{
|
||||
return full_name;
|
||||
}
|
||||
}
|
||||
}
|
||||
return base_name;
|
||||
}
|
||||
|
||||
void DWBLocalPlanner::loadCritics(const std::string name)
|
||||
{
|
||||
planner_nh_.param("default_critic_namespaces", default_critic_namespaces_);
|
||||
if (default_critic_namespaces_.size() == 0)
|
||||
{
|
||||
default_critic_namespaces_.push_back("dwb_critics");
|
||||
}
|
||||
|
||||
if (!planner_nh_.hasParam("critics"))
|
||||
{
|
||||
loadBackwardsCompatibleParameters(planner_nh_);
|
||||
}
|
||||
|
||||
std::vector<std::string> critic_names;
|
||||
planner_nh_.getParam("critics", critic_names);
|
||||
for (unsigned int i = 0; i < critic_names.size(); i++)
|
||||
{
|
||||
std::string plugin_name = critic_names[i];
|
||||
std::string plugin_class;
|
||||
planner_nh_.param(plugin_name + "/class", plugin_class, plugin_name);
|
||||
plugin_class = resolveCriticClassName(plugin_class);
|
||||
|
||||
TrajectoryCritic::Ptr plugin = std::move(critic_loader_.createUniqueInstance(plugin_class));
|
||||
ROS_INFO_NAMED("DWBLocalPlanner", "Using critic \"%s\" (%s)", plugin_name.c_str(), plugin_class.c_str());
|
||||
critics_.push_back(plugin);
|
||||
plugin->initialize(planner_nh_, plugin_name, costmap_);
|
||||
}
|
||||
}
|
||||
|
||||
bool DWBLocalPlanner::isGoalReached(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity)
|
||||
{
|
||||
if (goal_pose_.header.frame_id == "")
|
||||
{
|
||||
ROS_WARN_NAMED("DWBLocalPlanner", "Cannot check if the goal is reached without the goal being set!");
|
||||
return false;
|
||||
}
|
||||
|
||||
// Update time stamp of goal pose
|
||||
goal_pose_.header.stamp = pose.header.stamp;
|
||||
|
||||
bool ret = goal_checker_->isGoalReached(transformPoseToLocal(pose), transformPoseToLocal(goal_pose_), velocity);
|
||||
|
||||
if (ret)
|
||||
{
|
||||
ROS_INFO_THROTTLE_NAMED(1.0, "DWBLocalPlanner", "Goal reached!");
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
void DWBLocalPlanner::setGoalPose(const nav_2d_msgs::Pose2DStamped &goal_pose)
|
||||
{
|
||||
ROS_INFO_NAMED("DWBLocalPlanner", "New Goal Received.");
|
||||
goal_pose_ = goal_pose;
|
||||
traj_generator_->reset();
|
||||
goal_checker_->reset();
|
||||
for (TrajectoryCritic::Ptr critic : critics_)
|
||||
{
|
||||
critic->reset();
|
||||
}
|
||||
}
|
||||
|
||||
void DWBLocalPlanner::setPlan(const nav_2d_msgs::Path2D &path)
|
||||
{
|
||||
pub_.publishGlobalPlan(path);
|
||||
global_plan_ = path;
|
||||
}
|
||||
|
||||
nav_2d_msgs::Twist2DStamped DWBLocalPlanner::computeVelocityCommands(const nav_2d_msgs::Pose2DStamped &pose,
|
||||
// fr-> 102 LocalPlannerAdapter::computeVelocityCommands(..)
|
||||
const nav_2d_msgs::Twist2D &velocity)
|
||||
{
|
||||
std::shared_ptr<dwb_msgs::LocalPlanEvaluation> results = nullptr;
|
||||
if (pub_.shouldRecordEvaluation())
|
||||
{
|
||||
results = std::make_shared<dwb_msgs::LocalPlanEvaluation>();
|
||||
}
|
||||
|
||||
try
|
||||
{ // to -> 227 - DWBLocalPlanner::computeVelocityCommands(...)
|
||||
nav_2d_msgs::Twist2DStamped cmd_vel = computeVelocityCommands(pose, velocity, results);
|
||||
pub_.publishEvaluation(results);
|
||||
return cmd_vel;
|
||||
}
|
||||
catch (const nav_core2::PlannerException &e)
|
||||
{
|
||||
pub_.publishEvaluation(results);
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
void DWBLocalPlanner::prepare(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity)
|
||||
{
|
||||
if (update_costmap_before_planning_)
|
||||
{
|
||||
costmap_->update();
|
||||
}
|
||||
nav_2d_msgs::Path2D transformed_plan = transformGlobalPlan(pose);
|
||||
pub_.publishTransformedPlan(transformed_plan);
|
||||
|
||||
// Update time stamp of goal pose
|
||||
goal_pose_.header.stamp = pose.header.stamp;
|
||||
|
||||
geometry_msgs::Pose2D local_start_pose = transformPoseToLocal(pose),
|
||||
local_goal_pose = transformPoseToLocal(goal_pose_);
|
||||
|
||||
pub_.publishInputParams(costmap_->getInfo(), local_start_pose, velocity, local_goal_pose);
|
||||
|
||||
for (TrajectoryCritic::Ptr critic : critics_)
|
||||
{
|
||||
if (!critic->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan))
|
||||
{
|
||||
ROS_WARN_NAMED("DWBLocalPlanner", "Critic \"%s\" failed to prepare", critic->getName().c_str());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
nav_2d_msgs::Twist2DStamped DWBLocalPlanner::computeVelocityCommands(const nav_2d_msgs::Pose2DStamped &pose,
|
||||
// fr -> 179 DWBLocalPlanner::computeVelocityCommands(
|
||||
const nav_2d_msgs::Twist2D &velocity, std::shared_ptr<dwb_msgs::LocalPlanEvaluation> &results)
|
||||
{
|
||||
if (results)
|
||||
{
|
||||
results->header.frame_id = pose.header.frame_id;
|
||||
results->header.stamp = ros::Time::now();
|
||||
}
|
||||
prepare(pose, velocity);
|
||||
|
||||
try
|
||||
{
|
||||
// to 282 -> DWBLocalPlanner::coreScoringAlgorithm()
|
||||
dwb_msgs::TrajectoryScore best = coreScoringAlgorithm(pose.pose, velocity, results);
|
||||
|
||||
// Return Value
|
||||
nav_2d_msgs::Twist2DStamped cmd_vel;
|
||||
cmd_vel.header.stamp = ros::Time::now();
|
||||
cmd_vel.velocity = best.traj.velocity;
|
||||
|
||||
// debrief stateful critics
|
||||
for (TrajectoryCritic::Ptr critic : critics_)
|
||||
{
|
||||
critic->debrief(cmd_vel.velocity);
|
||||
}
|
||||
|
||||
pub_.publishLocalPlan(pose.header, best.traj);
|
||||
pub_.publishCostGrid(costmap_, critics_);
|
||||
|
||||
return cmd_vel;
|
||||
}
|
||||
catch (const NoLegalTrajectoriesException &e)
|
||||
{
|
||||
nav_2d_msgs::Twist2D empty_cmd;
|
||||
dwb_msgs::Trajectory2D empty_traj;
|
||||
// debrief stateful scoring functions
|
||||
for (TrajectoryCritic::Ptr critic : critics_)
|
||||
{
|
||||
critic->debrief(empty_cmd);
|
||||
}
|
||||
pub_.publishLocalPlan(pose.header, empty_traj);
|
||||
pub_.publishCostGrid(costmap_, critics_);
|
||||
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
dwb_msgs::TrajectoryScore DWBLocalPlanner::coreScoringAlgorithm(const geometry_msgs::Pose2D &pose,
|
||||
// fr -> 249 dwb_msgs::TrajectoryScore best = coreScoringAlgorithm(pose.pose, velocity, results);
|
||||
const nav_2d_msgs::Twist2D velocity,
|
||||
std::shared_ptr<dwb_msgs::LocalPlanEvaluation> &results)
|
||||
{
|
||||
nav_2d_msgs::Twist2D twist;
|
||||
dwb_msgs::Trajectory2D traj;
|
||||
dwb_msgs::TrajectoryScore best, worst;
|
||||
best.total = -1;
|
||||
worst.total = -1;
|
||||
IllegalTrajectoryTracker tracker;
|
||||
traj_generator_->startNewIteration(velocity); // nhận tốc độ hiện tại từ odom
|
||||
|
||||
while (traj_generator_->hasMoreTwists()) // return current_ > max_vel_ + EPSILON;
|
||||
{
|
||||
twist = traj_generator_->nextTwist();
|
||||
|
||||
traj = traj_generator_->generateTrajectory(pose, velocity, twist);
|
||||
|
||||
try
|
||||
{
|
||||
dwb_msgs::TrajectoryScore score = scoreTrajectory(traj, best.total);
|
||||
|
||||
tracker.addLegalTrajectory();
|
||||
if (results)
|
||||
{
|
||||
results->twists.push_back(score);
|
||||
}
|
||||
if (best.total < 0 || score.total < best.total)
|
||||
{
|
||||
best = score;
|
||||
if (results)
|
||||
{
|
||||
results->best_index = results->twists.size() - 1;
|
||||
}
|
||||
}
|
||||
if (worst.total < 0 || score.total > worst.total)
|
||||
{
|
||||
worst = score;
|
||||
if (results)
|
||||
{
|
||||
results->worst_index = results->twists.size() - 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
catch (const nav_core2::IllegalTrajectoryException &e)
|
||||
{
|
||||
if (results)
|
||||
{
|
||||
dwb_msgs::TrajectoryScore failed_score;
|
||||
failed_score.traj = traj;
|
||||
|
||||
dwb_msgs::CriticScore cs;
|
||||
cs.name = e.getCriticName();
|
||||
cs.raw_score = -1.0;
|
||||
failed_score.scores.push_back(cs);
|
||||
failed_score.total = -1.0;
|
||||
results->twists.push_back(failed_score);
|
||||
}
|
||||
tracker.addIllegalTrajectory(e);
|
||||
}
|
||||
}
|
||||
|
||||
if (best.total < 0)
|
||||
{
|
||||
if (debug_trajectory_details_)
|
||||
{
|
||||
ROS_ERROR_NAMED("DWBLocalPlanner", "%s", tracker.getMessage().c_str());
|
||||
for (auto const &x : tracker.getPercentages())
|
||||
{
|
||||
ROS_ERROR_NAMED("DWBLocalPlanner", "%.2f: %10s/%s", x.second, x.first.first.c_str(), x.first.second.c_str());
|
||||
}
|
||||
}
|
||||
throw NoLegalTrajectoriesException(tracker);
|
||||
}
|
||||
nav_msgs::Path pathMsg;
|
||||
pathMsg.header.frame_id = "odom";
|
||||
for (const auto& pose2D : traj.poses) {
|
||||
geometry_msgs::PoseStamped pose;
|
||||
pose.header.stamp = ros::Time::now(); // You can set the timestamp as needed
|
||||
pose.pose.position.x = pose2D.x;
|
||||
pose.pose.position.y = pose2D.y;
|
||||
tf2::Quaternion temp;
|
||||
temp.setRPY(0, 0, pose2D.theta);
|
||||
pose.pose.orientation.x = temp.getX();
|
||||
pose.pose.orientation.y = temp.getY();
|
||||
pose.pose.orientation.z = temp.getZ();
|
||||
pose.pose.orientation.w = temp.getW();
|
||||
// You may need to set other fields like orientation if needed.
|
||||
|
||||
pathMsg.poses.push_back(pose);
|
||||
}
|
||||
traj_pub_.publish(pathMsg);
|
||||
return best;
|
||||
}
|
||||
|
||||
dwb_msgs::TrajectoryScore DWBLocalPlanner::scoreTrajectory(const dwb_msgs::Trajectory2D &traj,
|
||||
double best_score)
|
||||
{
|
||||
dwb_msgs::TrajectoryScore score;
|
||||
score.traj = traj;
|
||||
|
||||
for (TrajectoryCritic::Ptr critic : critics_)
|
||||
{
|
||||
dwb_msgs::CriticScore cs;
|
||||
cs.name = critic->getName();
|
||||
cs.scale = critic->getScale();
|
||||
|
||||
if (cs.scale == 0.0)
|
||||
{
|
||||
score.scores.push_back(cs);
|
||||
continue;
|
||||
}
|
||||
double critic_score = critic->scoreTrajectory(traj);
|
||||
cs.raw_score = critic_score;
|
||||
score.scores.push_back(cs);
|
||||
score.total += critic_score * cs.scale;
|
||||
if (short_circuit_trajectory_evaluation_ && best_score > 0 && score.total > best_score)
|
||||
{
|
||||
// since we keep adding positives, once we are worse than the best, we will stay worse
|
||||
break;
|
||||
}
|
||||
}
|
||||
return score;
|
||||
}
|
||||
|
||||
double getSquareDistance(const geometry_msgs::Pose2D &pose_a, const geometry_msgs::Pose2D &pose_b)
|
||||
{
|
||||
double x_diff = pose_a.x - pose_b.x;
|
||||
double y_diff = pose_a.y - pose_b.y;
|
||||
return x_diff * x_diff + y_diff * y_diff;
|
||||
}
|
||||
|
||||
nav_2d_msgs::Path2D DWBLocalPlanner::transformGlobalPlan(const nav_2d_msgs::Pose2DStamped &pose)
|
||||
{
|
||||
|
||||
nav_2d_msgs::Path2D transformed_plan;
|
||||
if (global_plan_.poses.size() == 0)
|
||||
{
|
||||
throw nav_core2::PlannerException("Received plan with zero length");
|
||||
}
|
||||
|
||||
// let's get the pose of the robot in the frame of the plan
|
||||
nav_2d_msgs::Pose2DStamped robot_pose;
|
||||
if (!nav_2d_utils::transformPose(tf_, global_plan_.header.frame_id, pose, robot_pose))
|
||||
{
|
||||
throw nav_core2::PlannerTFException("Unable to transform robot pose into global plan's frame");
|
||||
}
|
||||
|
||||
transformed_plan.header.frame_id = costmap_->getFrameId();
|
||||
transformed_plan.header.stamp = pose.header.stamp;
|
||||
|
||||
// we'll discard points on the plan that are outside the local costmap
|
||||
double dist_threshold = std::max(costmap_->getWidth(), costmap_->getHeight()) * costmap_->getResolution() / 2.0;
|
||||
double sq_dist_threshold = dist_threshold * dist_threshold;
|
||||
nav_2d_msgs::Pose2DStamped stamped_pose;
|
||||
stamped_pose.header.frame_id = global_plan_.header.frame_id;
|
||||
|
||||
for (unsigned int i = 0; i < global_plan_.poses.size(); i++)
|
||||
{
|
||||
bool should_break = false;
|
||||
if (getSquareDistance(robot_pose.pose, global_plan_.poses[i]) > sq_dist_threshold)
|
||||
{
|
||||
if (transformed_plan.poses.size() == 0)
|
||||
{
|
||||
// we need to skip to a point on the plan that is within a certain distance of the robot
|
||||
continue;
|
||||
}
|
||||
else
|
||||
{
|
||||
// we're done transforming points
|
||||
should_break = true;
|
||||
}
|
||||
}
|
||||
|
||||
// now we'll transform until points are outside of our distance threshold
|
||||
stamped_pose.pose = global_plan_.poses[i];
|
||||
transformed_plan.poses.push_back(transformPoseToLocal(stamped_pose));
|
||||
if (should_break)
|
||||
break;
|
||||
}
|
||||
|
||||
// Prune both plans based on robot position
|
||||
// Note that this part of the algorithm assumes that the global plan starts near the robot (at one point)
|
||||
// Otherwise it may take a few iterations to converge to the proper behavior
|
||||
if (prune_plan_)
|
||||
{
|
||||
// let's get the pose of the robot in the frame of the transformed_plan/costmap
|
||||
nav_2d_msgs::Pose2DStamped costmap_pose;
|
||||
if (!nav_2d_utils::transformPose(tf_, transformed_plan.header.frame_id, pose, costmap_pose))
|
||||
{
|
||||
throw nav_core2::PlannerTFException("Unable to transform robot pose into costmap's frame");
|
||||
}
|
||||
|
||||
ROS_ASSERT(global_plan_.poses.size() >= transformed_plan.poses.size());
|
||||
std::vector<geometry_msgs::Pose2D>::iterator it = transformed_plan.poses.begin();
|
||||
std::vector<geometry_msgs::Pose2D>::iterator global_it = global_plan_.poses.begin();
|
||||
double sq_prune_dist = prune_distance_ * prune_distance_;
|
||||
while (it != transformed_plan.poses.end())
|
||||
{
|
||||
const geometry_msgs::Pose2D &w = *it;
|
||||
// Fixed error bound of 1 meter for now. Can reduce to a portion of the map size or based on the resolution
|
||||
if (getSquareDistance(costmap_pose.pose, w) < sq_prune_dist)
|
||||
{
|
||||
ROS_DEBUG_NAMED("DWBLocalPlanner", "Nearest waypoint to <%f, %f> is <%f, %f>\n",
|
||||
costmap_pose.pose.x, costmap_pose.pose.y, w.x, w.y);
|
||||
break;
|
||||
}
|
||||
it = transformed_plan.poses.erase(it);
|
||||
global_it = global_plan_.poses.erase(global_it);
|
||||
}
|
||||
pub_.publishGlobalPlan(global_plan_);
|
||||
}
|
||||
|
||||
if (transformed_plan.poses.size() == 0)
|
||||
{
|
||||
throw nav_core2::PlannerException("Resulting plan has 0 poses in it.");
|
||||
}
|
||||
return transformed_plan;
|
||||
}
|
||||
|
||||
geometry_msgs::Pose2D DWBLocalPlanner::transformPoseToLocal(const nav_2d_msgs::Pose2DStamped &pose)
|
||||
{
|
||||
return nav_2d_utils::transformStampedPose(tf_, pose, costmap_->getFrameId());
|
||||
}
|
||||
|
||||
} // namespace dwb_local_planner
|
||||
|
||||
// register this planner as a LocalPlanner plugin
|
||||
PLUGINLIB_EXPORT_CLASS(dwb_local_planner::DWBLocalPlanner, nav_core2::LocalPlanner)
|
||||
82
navigations/dwb_local_planner/src/illegal_trajectory_tracker.cpp
Executable file
82
navigations/dwb_local_planner/src/illegal_trajectory_tracker.cpp
Executable file
@@ -0,0 +1,82 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include <dwb_local_planner/illegal_trajectory_tracker.h>
|
||||
#include <map>
|
||||
#include <utility>
|
||||
#include <string>
|
||||
#include <sstream>
|
||||
|
||||
namespace dwb_local_planner
|
||||
{
|
||||
void IllegalTrajectoryTracker::addIllegalTrajectory(const nav_core2::IllegalTrajectoryException& e)
|
||||
{
|
||||
counts_[std::make_pair(e.getCriticName(), e.what())]++;
|
||||
illegal_count_++;
|
||||
}
|
||||
|
||||
void IllegalTrajectoryTracker::addLegalTrajectory()
|
||||
{
|
||||
legal_count_++;
|
||||
}
|
||||
|
||||
std::map< std::pair<std::string, std::string>, double> IllegalTrajectoryTracker::getPercentages() const
|
||||
{
|
||||
std::map< std::pair<std::string, std::string>, double> percents;
|
||||
double denominator = static_cast<double>(legal_count_ + illegal_count_);
|
||||
for (auto const& x : counts_)
|
||||
{
|
||||
percents[x.first] = static_cast<double>(x.second) / denominator;
|
||||
}
|
||||
return percents;
|
||||
}
|
||||
|
||||
std::string IllegalTrajectoryTracker::getMessage() const
|
||||
{
|
||||
std::ostringstream msg;
|
||||
if (legal_count_ == 0)
|
||||
{
|
||||
msg << "No valid trajectories out of " << illegal_count_ << "! ";
|
||||
}
|
||||
else
|
||||
{
|
||||
unsigned int total = legal_count_ + illegal_count_;
|
||||
msg << legal_count_ << " valid trajectories found (";
|
||||
msg << static_cast<double>(100 * legal_count_) / static_cast<double>(total);
|
||||
msg << "% of " << total << "). ";
|
||||
}
|
||||
return msg.str();
|
||||
}
|
||||
|
||||
} // namespace dwb_local_planner
|
||||
60
navigations/dwb_local_planner/src/planner_node.cpp
Executable file
60
navigations/dwb_local_planner/src/planner_node.cpp
Executable file
@@ -0,0 +1,60 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
#include <ros/ros.h>
|
||||
#include <pluginlib/class_loader.h>
|
||||
#include <dwb_local_planner/debug_dwb_local_planner.h>
|
||||
#include <tf2_ros/transform_listener.h>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
ros::init(argc, argv, "plan_node");
|
||||
ros::NodeHandle private_nh("~");
|
||||
|
||||
dwb_local_planner::DebugDWBLocalPlanner planner;
|
||||
ROS_INFO("Plan Node");
|
||||
|
||||
TFListenerPtr tf = std::make_shared<tf2_ros::Buffer>();
|
||||
tf2_ros::TransformListener tf2(*tf);
|
||||
|
||||
pluginlib::ClassLoader<nav_core2::Costmap> costmap_loader("nav_core2", "nav_core2::Costmap");
|
||||
std::string costmap_class;
|
||||
private_nh.param("local_costmap_class", costmap_class, std::string("nav_core_adapter::CostmapAdapter"));
|
||||
nav_core2::Costmap::Ptr costmap = costmap_loader.createUniqueInstance(costmap_class);
|
||||
costmap->initialize(private_nh, "costmap", tf);
|
||||
|
||||
planner.initialize(private_nh, "dwb_local_planner", tf, costmap);
|
||||
ros::spin();
|
||||
}
|
||||
246
navigations/dwb_local_planner/src/publisher.cpp
Executable file
246
navigations/dwb_local_planner/src/publisher.cpp
Executable file
@@ -0,0 +1,246 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2017, Locus Robotics
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
#include <dwb_local_planner/publisher.h>
|
||||
#include <nav_grid/coordinate_conversion.h>
|
||||
#include <visualization_msgs/Marker.h>
|
||||
#include <visualization_msgs/MarkerArray.h>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
#include <sensor_msgs/point_cloud_conversion.h>
|
||||
#include <nav_2d_utils/conversions.h>
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
|
||||
namespace dwb_local_planner
|
||||
{
|
||||
|
||||
void DWBPublisher::initialize(ros::NodeHandle& nh)
|
||||
{
|
||||
ros::NodeHandle global_nh;
|
||||
// Load Publishers
|
||||
nh.param("publish_evaluation", publish_evaluation_, true);
|
||||
if (publish_evaluation_)
|
||||
eval_pub_ = nh.advertise<dwb_msgs::LocalPlanEvaluation>("evaluation", 1);
|
||||
|
||||
nh.param("publish_input_params", publish_input_params_, true);
|
||||
if (publish_input_params_)
|
||||
{
|
||||
info_pub_ = nh.advertise<nav_2d_msgs::NavGridInfo>("info", 1);
|
||||
pose_pub_ = nh.advertise<geometry_msgs::Pose2D>("pose", 1);
|
||||
goal_pub_ = nh.advertise<geometry_msgs::Pose2D>("goal", 1);
|
||||
velocity_pub_ = nh.advertise<nav_2d_msgs::Twist2D>("velocity", 1);
|
||||
}
|
||||
|
||||
nh.param("publish_global_plan", publish_global_plan_, true);
|
||||
if (publish_global_plan_)
|
||||
global_pub_ = nh.advertise<nav_msgs::Path>("global_plan", 1);
|
||||
|
||||
nh.param("publish_transformed_plan", publish_transformed_, true);
|
||||
if (publish_transformed_)
|
||||
transformed_pub_ = nh.advertise<nav_msgs::Path>("transformed_global_plan", 1);
|
||||
|
||||
nh.param("publish_local_plan", publish_local_plan_, true);
|
||||
if (publish_local_plan_)
|
||||
local_pub_ = nh.advertise<nav_msgs::Path>("local_plan", 1);
|
||||
|
||||
nh.param("publish_trajectories", publish_trajectories_, true);
|
||||
if (publish_trajectories_)
|
||||
marker_pub_ = global_nh.advertise<visualization_msgs::MarkerArray>("marker", 1);
|
||||
double marker_lifetime;
|
||||
nh.param("marker_lifetime", marker_lifetime, 0.1);
|
||||
marker_lifetime_ = ros::Duration(marker_lifetime);
|
||||
|
||||
nh.param("publish_cost_grid_pc", publish_cost_grid_pc_, false);
|
||||
if (publish_cost_grid_pc_)
|
||||
cost_grid_pc_pub_ = nh.advertise<sensor_msgs::PointCloud2>("cost_cloud", 1);
|
||||
}
|
||||
|
||||
void DWBPublisher::publishEvaluation(std::shared_ptr<dwb_msgs::LocalPlanEvaluation> results)
|
||||
{
|
||||
if (results == nullptr) return;
|
||||
if (publish_evaluation_ && eval_pub_.getNumSubscribers() > 0)
|
||||
{
|
||||
eval_pub_.publish(*results);
|
||||
}
|
||||
publishTrajectories(*results);
|
||||
}
|
||||
|
||||
void DWBPublisher::publishTrajectories(const dwb_msgs::LocalPlanEvaluation& results)
|
||||
{
|
||||
if (!publish_trajectories_ || marker_pub_.getNumSubscribers() == 0) return;
|
||||
visualization_msgs::MarkerArray ma;
|
||||
visualization_msgs::Marker m;
|
||||
|
||||
if (results.twists.size() == 0) return;
|
||||
|
||||
geometry_msgs::Point pt;
|
||||
|
||||
m.header = results.header;
|
||||
m.type = m.LINE_STRIP;
|
||||
m.pose.orientation.w = 1;
|
||||
m.scale.x = 0.002;
|
||||
m.color.a = 1.0;
|
||||
m.lifetime = marker_lifetime_;
|
||||
|
||||
double best_cost = results.twists[results.best_index].total,
|
||||
worst_cost = results.twists[results.worst_index].total,
|
||||
denominator = worst_cost - best_cost;
|
||||
if (std::fabs(denominator) < 1e-9)
|
||||
{
|
||||
denominator = 1.0;
|
||||
}
|
||||
|
||||
for (unsigned int i = 0; i < results.twists.size(); i++)
|
||||
{
|
||||
const dwb_msgs::TrajectoryScore& twist = results.twists[i];
|
||||
if (twist.total >= 0)
|
||||
{
|
||||
m.color.r = 1 - (twist.total - best_cost) / denominator;
|
||||
m.color.g = 1 - (twist.total - best_cost) / denominator;
|
||||
m.color.b = 1;
|
||||
m.ns = "ValidTrajectories";
|
||||
}
|
||||
else
|
||||
{
|
||||
m.color.b = 0;
|
||||
m.ns = "InvalidTrajectories";
|
||||
}
|
||||
m.points.clear();
|
||||
for (unsigned int j = 0; j < twist.traj.poses.size(); ++j)
|
||||
{
|
||||
pt.x = twist.traj.poses[j].x;
|
||||
pt.y = twist.traj.poses[j].y;
|
||||
pt.z = 0;
|
||||
m.points.push_back(pt);
|
||||
}
|
||||
ma.markers.push_back(m);
|
||||
m.id += 1;
|
||||
}
|
||||
|
||||
marker_pub_.publish(ma);
|
||||
}
|
||||
|
||||
void DWBPublisher::publishLocalPlan(const std_msgs::Header& header,
|
||||
const dwb_msgs::Trajectory2D& traj)
|
||||
{
|
||||
if (!publish_local_plan_ || local_pub_.getNumSubscribers() == 0) return;
|
||||
|
||||
nav_msgs::Path path = nav_2d_utils::poses2DToPath(traj.poses, header.frame_id, header.stamp);
|
||||
local_pub_.publish(path);
|
||||
}
|
||||
|
||||
void DWBPublisher::publishCostGrid(const nav_core2::Costmap::Ptr costmap,
|
||||
const std::vector<TrajectoryCritic::Ptr> critics)
|
||||
{
|
||||
if (!publish_cost_grid_pc_ || cost_grid_pc_pub_.getNumSubscribers() == 0) return;
|
||||
|
||||
const nav_grid::NavGridInfo& info = costmap->getInfo();
|
||||
sensor_msgs::PointCloud cost_grid_pc;
|
||||
cost_grid_pc.header.frame_id = info.frame_id;
|
||||
cost_grid_pc.header.stamp = ros::Time::now();
|
||||
|
||||
double x_coord, y_coord;
|
||||
unsigned int n = info.width * info.height;
|
||||
cost_grid_pc.points.resize(n);
|
||||
unsigned int i = 0;
|
||||
for (unsigned int cy = 0; cy < info.height; cy++)
|
||||
{
|
||||
for (unsigned int cx = 0; cx < info.width; cx++)
|
||||
{
|
||||
gridToWorld(info, cx, cy, x_coord, y_coord);
|
||||
cost_grid_pc.points[i].x = x_coord;
|
||||
cost_grid_pc.points[i].y = y_coord;
|
||||
i++;
|
||||
}
|
||||
}
|
||||
|
||||
sensor_msgs::ChannelFloat32 totals;
|
||||
totals.name = "total_cost";
|
||||
totals.values.resize(n);
|
||||
|
||||
for (TrajectoryCritic::Ptr critic : critics)
|
||||
{
|
||||
unsigned int channel_index = cost_grid_pc.channels.size();
|
||||
critic->addCriticVisualization(cost_grid_pc);
|
||||
if (channel_index == cost_grid_pc.channels.size())
|
||||
{
|
||||
// No channels were added, so skip to next critic
|
||||
continue;
|
||||
}
|
||||
double scale = critic->getScale();
|
||||
for (i = 0; i < n; i++)
|
||||
{
|
||||
totals.values[i] += cost_grid_pc.channels[channel_index].values[i] * scale;
|
||||
}
|
||||
}
|
||||
cost_grid_pc.channels.push_back(totals);
|
||||
|
||||
sensor_msgs::PointCloud2 cost_grid_pc2;
|
||||
convertPointCloudToPointCloud2(cost_grid_pc, cost_grid_pc2);
|
||||
cost_grid_pc_pub_.publish(cost_grid_pc2);
|
||||
}
|
||||
|
||||
void DWBPublisher::publishGlobalPlan(const nav_2d_msgs::Path2D plan)
|
||||
{
|
||||
publishGenericPlan(plan, global_pub_, publish_global_plan_);
|
||||
}
|
||||
|
||||
void DWBPublisher::publishTransformedPlan(const nav_2d_msgs::Path2D plan)
|
||||
{
|
||||
publishGenericPlan(plan, transformed_pub_, publish_transformed_);
|
||||
}
|
||||
|
||||
void DWBPublisher::publishLocalPlan(const nav_2d_msgs::Path2D plan)
|
||||
{
|
||||
publishGenericPlan(plan, local_pub_, publish_local_plan_);
|
||||
}
|
||||
|
||||
void DWBPublisher::publishGenericPlan(const nav_2d_msgs::Path2D plan, const ros::Publisher pub, bool flag)
|
||||
{
|
||||
if (!flag || pub.getNumSubscribers() == 0) return;
|
||||
nav_msgs::Path path = nav_2d_utils::pathToPath(plan);
|
||||
pub.publish(path);
|
||||
}
|
||||
|
||||
void DWBPublisher::publishInputParams(const nav_grid::NavGridInfo& info, const geometry_msgs::Pose2D& start_pose,
|
||||
const nav_2d_msgs::Twist2D& velocity, const geometry_msgs::Pose2D& goal_pose)
|
||||
{
|
||||
if (!publish_input_params_) return;
|
||||
|
||||
info_pub_.publish(nav_2d_utils::toMsg(info));
|
||||
pose_pub_.publish(start_pose);
|
||||
goal_pub_.publish(goal_pose);
|
||||
velocity_pub_.publish(velocity);
|
||||
}
|
||||
|
||||
} // namespace dwb_local_planner
|
||||
105
navigations/dwb_local_planner/src/trajectory_utils.cpp
Executable file
105
navigations/dwb_local_planner/src/trajectory_utils.cpp
Executable file
@@ -0,0 +1,105 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2018, Locus Robotics
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include <dwb_local_planner/trajectory_utils.h>
|
||||
#include <nav_core2/exceptions.h>
|
||||
|
||||
namespace dwb_local_planner
|
||||
{
|
||||
const geometry_msgs::Pose2D& getClosestPose(const dwb_msgs::Trajectory2D& trajectory, const double time_offset)
|
||||
{
|
||||
ros::Duration goal_time(time_offset);
|
||||
const unsigned int num_poses = trajectory.poses.size();
|
||||
if (num_poses == 0)
|
||||
{
|
||||
throw nav_core2::PlannerException("Cannot call getClosestPose on empty trajectory.");
|
||||
}
|
||||
unsigned int closest_index = num_poses;
|
||||
double closest_diff = 0.0;
|
||||
for (unsigned int i=0; i < num_poses; ++i)
|
||||
{
|
||||
double diff = fabs((trajectory.time_offsets[i] - goal_time).toSec());
|
||||
if (closest_index == num_poses || diff < closest_diff)
|
||||
{
|
||||
closest_index = i;
|
||||
closest_diff = diff;
|
||||
}
|
||||
if (goal_time < trajectory.time_offsets[i])
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
return trajectory.poses[closest_index];
|
||||
}
|
||||
|
||||
geometry_msgs::Pose2D projectPose(const dwb_msgs::Trajectory2D& trajectory, const double time_offset)
|
||||
{
|
||||
ros::Duration goal_time(time_offset);
|
||||
const unsigned int num_poses = trajectory.poses.size();
|
||||
if (num_poses == 0)
|
||||
{
|
||||
throw nav_core2::PlannerException("Cannot call projectPose on empty trajectory.");
|
||||
}
|
||||
if (goal_time <= trajectory.time_offsets[0])
|
||||
{
|
||||
return trajectory.poses[0];
|
||||
}
|
||||
else if (goal_time >= trajectory.time_offsets[num_poses - 1])
|
||||
{
|
||||
return trajectory.poses[num_poses - 1];
|
||||
}
|
||||
|
||||
for (unsigned int i=0; i < num_poses - 1; ++i)
|
||||
{
|
||||
if (goal_time >= trajectory.time_offsets[i] && goal_time < trajectory.time_offsets[i+1])
|
||||
{
|
||||
double time_diff = (trajectory.time_offsets[i+1] - trajectory.time_offsets[i]).toSec();
|
||||
double ratio = (goal_time - trajectory.time_offsets[i]).toSec() / time_diff;
|
||||
double inv_ratio = 1.0 - ratio;
|
||||
const geometry_msgs::Pose2D& pose_a = trajectory.poses[i];
|
||||
const geometry_msgs::Pose2D& pose_b = trajectory.poses[i + 1];
|
||||
geometry_msgs::Pose2D projected;
|
||||
projected.x = pose_a.x * inv_ratio + pose_b.x * ratio;
|
||||
projected.y = pose_a.y * inv_ratio + pose_b.y * ratio;
|
||||
projected.theta = pose_a.theta * inv_ratio + pose_b.theta * ratio;
|
||||
return projected;
|
||||
}
|
||||
}
|
||||
|
||||
// Should not reach this point
|
||||
return trajectory.poses[num_poses - 1];
|
||||
}
|
||||
|
||||
|
||||
} // namespace dwb_local_planner
|
||||
74
navigations/dwb_local_planner/structure.drawio
Executable file
74
navigations/dwb_local_planner/structure.drawio
Executable file
@@ -0,0 +1,74 @@
|
||||
<mxfile host="Electron" modified="2023-09-19T04:10:13.524Z" agent="Mozilla/5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/21.7.5 Chrome/114.0.5735.289 Electron/25.8.1 Safari/537.36" etag="OqqRPxsWm9AdmGXybgqc" version="21.7.5" type="device">
|
||||
<diagram name="Page-1" id="2YBvvXClWsGukQMizWep">
|
||||
<mxGraphModel dx="628" dy="1425" grid="1" gridSize="10" guides="1" tooltips="1" connect="1" arrows="1" fold="1" page="1" pageScale="1" pageWidth="850" pageHeight="1100" math="0" shadow="0">
|
||||
<root>
|
||||
<mxCell id="0" />
|
||||
<mxCell id="1" parent="0" />
|
||||
<mxCell id="aM9ryv3xv72pqoxQDRHE-1" value="<div style="color: rgb(246, 246, 244); background-color: rgb(40, 42, 54); font-family: &quot;Droid Sans Mono&quot;, &quot;monospace&quot;, monospace; font-weight: normal; font-size: 14px; line-height: 19px;"><div><span style="color: #62e884;">computeVelocityCommands</span></div></div>" style="shape=umlLifeline;perimeter=lifelinePerimeter;whiteSpace=wrap;html=1;container=0;dropTarget=0;collapsible=0;recursiveResize=0;outlineConnect=0;portConstraint=eastwest;newEdgeStyle={"edgeStyle":"elbowEdgeStyle","elbow":"vertical","curved":0,"rounded":0};" parent="1" vertex="1">
|
||||
<mxGeometry x="210" y="110" width="180" height="260" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="aM9ryv3xv72pqoxQDRHE-2" value="" style="html=1;points=[];perimeter=orthogonalPerimeter;outlineConnect=0;targetShapes=umlLifeline;portConstraint=eastwest;newEdgeStyle={"edgeStyle":"elbowEdgeStyle","elbow":"vertical","curved":0,"rounded":0};" parent="aM9ryv3xv72pqoxQDRHE-1" vertex="1">
|
||||
<mxGeometry x="45" y="70" width="10" height="190" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="aM9ryv3xv72pqoxQDRHE-3" value="<div style="color: rgb(246, 246, 244); background-color: rgb(40, 42, 54); font-family: &quot;Droid Sans Mono&quot;, &quot;monospace&quot;, monospace; line-height: 19px;"><font style="font-size: 10px;">geometry_msgs::<span style="color: rgb(151, 225, 241); font-style: italic;">Twist</span><span style="color: rgb(242, 134, 196);">&amp;</span>&nbsp;</font></div><div style="color: rgb(246, 246, 244); background-color: rgb(40, 42, 54); font-family: &quot;Droid Sans Mono&quot;, &quot;monospace&quot;, monospace; line-height: 19px;"><font style="font-size: 10px;"><span style="color: rgb(255, 184, 108); font-style: italic;">cmd_vel</span></font></div>" style="html=1;verticalAlign=bottom;startArrow=oval;endArrow=block;startSize=8;edgeStyle=elbowEdgeStyle;elbow=vertical;curved=0;rounded=0;" parent="aM9ryv3xv72pqoxQDRHE-1" target="aM9ryv3xv72pqoxQDRHE-2" edge="1">
|
||||
<mxGeometry x="-0.8788" y="10" relative="1" as="geometry">
|
||||
<mxPoint x="-120" y="70" as="sourcePoint" />
|
||||
<mxPoint as="offset" />
|
||||
</mxGeometry>
|
||||
</mxCell>
|
||||
<mxCell id="aM9ryv3xv72pqoxQDRHE-4" value="" style="html=1;points=[];perimeter=orthogonalPerimeter;outlineConnect=0;targetShapes=umlLifeline;portConstraint=eastwest;newEdgeStyle={"edgeStyle":"elbowEdgeStyle","elbow":"vertical","curved":0,"rounded":0};" parent="aM9ryv3xv72pqoxQDRHE-1" vertex="1">
|
||||
<mxGeometry x="50" y="120" width="10" height="80" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="aM9ryv3xv72pqoxQDRHE-5" value=":Object" style="shape=umlLifeline;perimeter=lifelinePerimeter;whiteSpace=wrap;html=1;container=0;dropTarget=0;collapsible=0;recursiveResize=0;outlineConnect=0;portConstraint=eastwest;newEdgeStyle={"edgeStyle":"elbowEdgeStyle","elbow":"vertical","curved":0,"rounded":0};" parent="1" vertex="1">
|
||||
<mxGeometry x="473" y="110" width="100" height="300" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="aM9ryv3xv72pqoxQDRHE-6" value="" style="html=1;points=[];perimeter=orthogonalPerimeter;outlineConnect=0;targetShapes=umlLifeline;portConstraint=eastwest;newEdgeStyle={"edgeStyle":"elbowEdgeStyle","elbow":"vertical","curved":0,"rounded":0};" parent="aM9ryv3xv72pqoxQDRHE-5" vertex="1">
|
||||
<mxGeometry x="45" y="80" width="10" height="170" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="aM9ryv3xv72pqoxQDRHE-10" value="return" style="html=1;verticalAlign=bottom;endArrow=open;dashed=1;endSize=8;edgeStyle=elbowEdgeStyle;elbow=vertical;curved=0;rounded=0;" parent="1" source="aM9ryv3xv72pqoxQDRHE-4" target="aM9ryv3xv72pqoxQDRHE-6" edge="1">
|
||||
<mxGeometry relative="1" as="geometry">
|
||||
<mxPoint x="428" y="375" as="targetPoint" />
|
||||
<Array as="points">
|
||||
<mxPoint x="433" y="370" />
|
||||
</Array>
|
||||
</mxGeometry>
|
||||
</mxCell>
|
||||
<mxCell id="A1EfnVpFrXilNoYTl5xU-1" value="nav_core_adapter::<br>LocalPlannerAdapter" style="html=1;whiteSpace=wrap;" vertex="1" parent="1">
|
||||
<mxGeometry x="230" y="20" width="140" height="50" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="A1EfnVpFrXilNoYTl5xU-2" value="Use" style="endArrow=open;endSize=12;dashed=1;html=1;rounded=0;exitX=1;exitY=0.5;exitDx=0;exitDy=0;" edge="1" parent="1" source="A1EfnVpFrXilNoYTl5xU-1">
|
||||
<mxGeometry width="160" relative="1" as="geometry">
|
||||
<mxPoint x="380" y="44.58" as="sourcePoint" />
|
||||
<mxPoint x="490" y="44.92" as="targetPoint" />
|
||||
</mxGeometry>
|
||||
</mxCell>
|
||||
<mxCell id="A1EfnVpFrXilNoYTl5xU-3" value="" style="endArrow=block;startArrow=block;endFill=1;startFill=1;html=1;rounded=0;" edge="1" parent="1" target="aM9ryv3xv72pqoxQDRHE-5">
|
||||
<mxGeometry width="160" relative="1" as="geometry">
|
||||
<mxPoint x="270" y="270" as="sourcePoint" />
|
||||
<mxPoint x="430" y="270" as="targetPoint" />
|
||||
</mxGeometry>
|
||||
</mxCell>
|
||||
<mxCell id="A1EfnVpFrXilNoYTl5xU-4" value="reference" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" vertex="1" connectable="0" parent="A1EfnVpFrXilNoYTl5xU-3">
|
||||
<mxGeometry x="-0.0205" y="3" relative="1" as="geometry">
|
||||
<mxPoint as="offset" />
|
||||
</mxGeometry>
|
||||
</mxCell>
|
||||
<mxCell id="A1EfnVpFrXilNoYTl5xU-6" value="name" style="endArrow=block;endFill=1;html=1;edgeStyle=orthogonalEdgeStyle;align=left;verticalAlign=top;rounded=0;" edge="1" parent="1">
|
||||
<mxGeometry x="-1" relative="1" as="geometry">
|
||||
<mxPoint x="290" y="330" as="sourcePoint" />
|
||||
<mxPoint x="450" y="330" as="targetPoint" />
|
||||
</mxGeometry>
|
||||
</mxCell>
|
||||
<mxCell id="A1EfnVpFrXilNoYTl5xU-7" value="1" style="edgeLabel;resizable=0;html=1;align=left;verticalAlign=bottom;" connectable="0" vertex="1" parent="A1EfnVpFrXilNoYTl5xU-6">
|
||||
<mxGeometry x="-1" relative="1" as="geometry" />
|
||||
</mxCell>
|
||||
<mxCell id="A1EfnVpFrXilNoYTl5xU-8" value="" style="endArrow=open;startArrow=circlePlus;endFill=0;startFill=0;endSize=8;html=1;rounded=0;" edge="1" parent="1">
|
||||
<mxGeometry width="160" relative="1" as="geometry">
|
||||
<mxPoint x="290" y="330" as="sourcePoint" />
|
||||
<mxPoint x="450" y="330" as="targetPoint" />
|
||||
</mxGeometry>
|
||||
</mxCell>
|
||||
</root>
|
||||
</mxGraphModel>
|
||||
</diagram>
|
||||
</mxfile>
|
||||
115
navigations/dwb_local_planner/test/utils_test.cpp
Executable file
115
navigations/dwb_local_planner/test/utils_test.cpp
Executable file
@@ -0,0 +1,115 @@
|
||||
/*
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2018, Locus Robotics
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the copyright holder nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <dwb_local_planner/trajectory_utils.h>
|
||||
|
||||
using dwb_local_planner::getClosestPose;
|
||||
using dwb_local_planner::projectPose;
|
||||
|
||||
TEST(Utils, ClosestPose)
|
||||
{
|
||||
dwb_msgs::Trajectory2D traj;
|
||||
traj.poses.resize(4);
|
||||
traj.time_offsets.resize(4);
|
||||
for (unsigned int i=0; i < traj.poses.size(); i++)
|
||||
{
|
||||
double d = static_cast<double>(i);
|
||||
traj.poses[i].x = d;
|
||||
traj.time_offsets[i] = ros::Duration(d);
|
||||
}
|
||||
|
||||
EXPECT_DOUBLE_EQ(getClosestPose(traj, 0.0).x, traj.poses[0].x);
|
||||
EXPECT_DOUBLE_EQ(getClosestPose(traj, -1.0).x, traj.poses[0].x);
|
||||
EXPECT_DOUBLE_EQ(getClosestPose(traj, 0.4).x, traj.poses[0].x);
|
||||
EXPECT_DOUBLE_EQ(getClosestPose(traj, 0.5).x, traj.poses[0].x);
|
||||
EXPECT_DOUBLE_EQ(getClosestPose(traj, 0.51).x, traj.poses[1].x);
|
||||
EXPECT_DOUBLE_EQ(getClosestPose(traj, 1.0).x, traj.poses[1].x);
|
||||
EXPECT_DOUBLE_EQ(getClosestPose(traj, 1.4999).x, traj.poses[1].x);
|
||||
EXPECT_DOUBLE_EQ(getClosestPose(traj, 2.0).x, traj.poses[2].x);
|
||||
EXPECT_DOUBLE_EQ(getClosestPose(traj, 2.51).x, traj.poses[3].x);
|
||||
EXPECT_DOUBLE_EQ(getClosestPose(traj, 3.5).x, traj.poses[3].x);
|
||||
}
|
||||
|
||||
TEST(Utils, ProjectPose)
|
||||
{
|
||||
dwb_msgs::Trajectory2D traj;
|
||||
traj.poses.resize(4);
|
||||
traj.time_offsets.resize(4);
|
||||
for (unsigned int i=0; i < traj.poses.size(); i++)
|
||||
{
|
||||
double d = static_cast<double>(i);
|
||||
traj.poses[i].x = d;
|
||||
traj.poses[i].y = 30.0 - 2.0 * d;
|
||||
traj.poses[i].theta = 0.42;
|
||||
traj.time_offsets[i] = ros::Duration(d);
|
||||
}
|
||||
|
||||
EXPECT_DOUBLE_EQ(projectPose(traj, 0.0).x, 0.0);
|
||||
EXPECT_DOUBLE_EQ(projectPose(traj, 0.0).y, 30.0);
|
||||
EXPECT_DOUBLE_EQ(projectPose(traj, 0.0).theta, 0.42);
|
||||
EXPECT_DOUBLE_EQ(projectPose(traj, -1.0).x, 0.0);
|
||||
EXPECT_DOUBLE_EQ(projectPose(traj, -1.0).y, 30.0);
|
||||
EXPECT_DOUBLE_EQ(projectPose(traj, -1.0).theta, 0.42);
|
||||
EXPECT_DOUBLE_EQ(projectPose(traj, 0.4).x, 0.4);
|
||||
EXPECT_DOUBLE_EQ(projectPose(traj, 0.4).y, 29.2);
|
||||
EXPECT_DOUBLE_EQ(projectPose(traj, 0.4).theta, 0.42);
|
||||
EXPECT_DOUBLE_EQ(projectPose(traj, 0.5).x, 0.5);
|
||||
EXPECT_DOUBLE_EQ(projectPose(traj, 0.5).y, 29.0);
|
||||
EXPECT_DOUBLE_EQ(projectPose(traj, 0.5).theta, 0.42);
|
||||
EXPECT_DOUBLE_EQ(projectPose(traj, 0.51).x, 0.51);
|
||||
EXPECT_DOUBLE_EQ(projectPose(traj, 0.51).y, 28.98);
|
||||
EXPECT_DOUBLE_EQ(projectPose(traj, 0.51).theta, 0.42);
|
||||
EXPECT_DOUBLE_EQ(projectPose(traj, 1.0).x, 1.0);
|
||||
EXPECT_DOUBLE_EQ(projectPose(traj, 1.0).y, 28.0);
|
||||
EXPECT_DOUBLE_EQ(projectPose(traj, 1.0).theta, 0.42);
|
||||
EXPECT_DOUBLE_EQ(projectPose(traj, 1.4999).x, 1.4999);
|
||||
EXPECT_DOUBLE_EQ(projectPose(traj, 1.4999).y, 27.0002);
|
||||
EXPECT_DOUBLE_EQ(projectPose(traj, 1.4999).theta, 0.42);
|
||||
EXPECT_DOUBLE_EQ(projectPose(traj, 2.0).x, 2.0);
|
||||
EXPECT_DOUBLE_EQ(projectPose(traj, 2.0).y, 26.0);
|
||||
EXPECT_DOUBLE_EQ(projectPose(traj, 2.0).theta, 0.42);
|
||||
EXPECT_FLOAT_EQ(projectPose(traj, 2.51).x, 2.51);
|
||||
EXPECT_FLOAT_EQ(projectPose(traj, 2.51).y, 24.98);
|
||||
EXPECT_DOUBLE_EQ(projectPose(traj, 2.51).theta, 0.42);
|
||||
EXPECT_DOUBLE_EQ(projectPose(traj, 3.5).x, 3.0);
|
||||
EXPECT_DOUBLE_EQ(projectPose(traj, 3.5).y, 24.0);
|
||||
EXPECT_DOUBLE_EQ(projectPose(traj, 3.5).theta, 0.42);
|
||||
}
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
return RUN_ALL_TESTS();
|
||||
}
|
||||
Reference in New Issue
Block a user