From 42d894fed50eb54144bad90c570fc928c3ff2743 Mon Sep 17 00:00:00 2001 From: TetsuKawa <70682030+TetsuKawa@users.noreply.github.com> Date: Mon, 11 Mar 2024 14:14:12 +0900 Subject: [PATCH] feat: add timeouts of request services (#6532) * feat: add timeouts of request services Signed-off-by: TetsuKawa * style(pre-commit): autofix * feat: replace define with enum Signed-off-by: TetsuKawa * style(pre-commit): autofix * modify: renam a function Signed-off-by: TetsuKawa * modify: rename a function Signed-off-by: TetsuKawa * modify: fix functions name at the caller side Signed-off-by: TetsuKawa --------- Signed-off-by: TetsuKawa Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Co-authored-by: Ryuta Kambe --- .../mrm_handler/config/mrm_handler.param.yaml | 2 + system/mrm_handler/image/mrm-state.svg | 232 +++++++++--------- .../include/mrm_handler/mrm_handler_core.hpp | 13 +- .../schema/mrm_handler.schema.json | 12 + .../src/mrm_handler/mrm_handler_core.cpp | 156 ++++++------ 5 files changed, 225 insertions(+), 190 deletions(-) diff --git a/system/mrm_handler/config/mrm_handler.param.yaml b/system/mrm_handler/config/mrm_handler.param.yaml index a0295fb07627c..e82ee36a7825a 100644 --- a/system/mrm_handler/config/mrm_handler.param.yaml +++ b/system/mrm_handler/config/mrm_handler.param.yaml @@ -4,6 +4,8 @@ ros__parameters: update_rate: 10 timeout_operation_mode_availability: 0.5 + timeout_call_mrm_behavior: 0.01 + timeout_cancel_mrm_behavior: 0.01 use_emergency_holding: false timeout_emergency_recovery: 5.0 use_parking_after_stopped: false diff --git a/system/mrm_handler/image/mrm-state.svg b/system/mrm_handler/image/mrm-state.svg index 13a2956b6f15c..dbcc13918feff 100644 --- a/system/mrm_handler/image/mrm-state.svg +++ b/system/mrm_handler/image/mrm-state.svg @@ -8,302 +8,300 @@ width="958px" height="341px" viewBox="-0.5 -0.5 958 341" - content="<mxfile host="app.diagrams.net" modified="2024-02-07T09:48:58.193Z" agent="Mozilla/5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) Chrome/118.0.0.0 Safari/537.36" etag="JzfBIbSx-UshJtjtpS4o" version="23.1.1" type="device"> <diagram id="C5RBs43oDa-KdzZeNtuy" name="Page-1"> <mxGraphModel dx="1364" dy="771" grid="1" gridSize="10" guides="1" tooltips="1" connect="1" arrows="1" fold="1" page="1" pageScale="1" pageWidth="1169" pageHeight="827" math="0" shadow="0"> <root> <mxCell id="WIyWlLk6GJQsqaUBKTNV-0" /> <mxCell id="WIyWlLk6GJQsqaUBKTNV-1" parent="WIyWlLk6GJQsqaUBKTNV-0" /> <mxCell id="6FWRLcRsoSDiZ6vq709X-284" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" edge="1" parent="WIyWlLk6GJQsqaUBKTNV-1" source="6FWRLcRsoSDiZ6vq709X-281" target="6FWRLcRsoSDiZ6vq709X-3"> <mxGeometry relative="1" as="geometry"> <Array as="points"> <mxPoint x="230" y="360" /> <mxPoint x="230" y="360" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-285" value="not emergency" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" vertex="1" connectable="0" parent="6FWRLcRsoSDiZ6vq709X-284"> <mxGeometry x="0.02" relative="1" as="geometry"> <mxPoint x="1" y="-10" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-281" value="" style="rounded=0;whiteSpace=wrap;html=1;" vertex="1" parent="WIyWlLk6GJQsqaUBKTNV-1"> <mxGeometry x="280" y="140" width="720" height="340" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-4" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=1;exitY=0.5;exitDx=0;exitDy=0;entryX=0;entryY=0.5;entryDx=0;entryDy=0;" edge="1" parent="WIyWlLk6GJQsqaUBKTNV-1" source="6FWRLcRsoSDiZ6vq709X-2" target="6FWRLcRsoSDiZ6vq709X-3"> <mxGeometry relative="1" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-2" value="" style="ellipse;whiteSpace=wrap;html=1;fillColor=#000000;" vertex="1" parent="WIyWlLk6GJQsqaUBKTNV-1"> <mxGeometry x="50" y="305" width="30" height="30" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-19" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=1;exitY=0.25;exitDx=0;exitDy=0;" edge="1" parent="WIyWlLk6GJQsqaUBKTNV-1" source="6FWRLcRsoSDiZ6vq709X-3" target="6FWRLcRsoSDiZ6vq709X-281"> <mxGeometry relative="1" as="geometry"> <mxPoint x="270" y="290" as="targetPoint" /> <Array as="points"> <mxPoint x="260" y="290" /> <mxPoint x="260" y="290" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-21" value="emergency" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" vertex="1" connectable="0" parent="6FWRLcRsoSDiZ6vq709X-19"> <mxGeometry x="-0.0719" y="-3" relative="1" as="geometry"> <mxPoint y="-13" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-3" value="&lt;b&gt;NORMAL&lt;/b&gt;" style="rounded=0;whiteSpace=wrap;html=1;" vertex="1" parent="WIyWlLk6GJQsqaUBKTNV-1"> <mxGeometry x="120" y="260" width="60" height="120" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-274" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" edge="1" parent="WIyWlLk6GJQsqaUBKTNV-1" source="6FWRLcRsoSDiZ6vq709X-5" target="6FWRLcRsoSDiZ6vq709X-272"> <mxGeometry relative="1" as="geometry"> <Array as="points"> <mxPoint x="810" y="260" /> <mxPoint x="810" y="260" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-275" value="succeeded" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" vertex="1" connectable="0" parent="6FWRLcRsoSDiZ6vq709X-274"> <mxGeometry x="-0.0667" y="-3" relative="1" as="geometry"> <mxPoint y="-13" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-278" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" edge="1" parent="WIyWlLk6GJQsqaUBKTNV-1" source="6FWRLcRsoSDiZ6vq709X-5" target="6FWRLcRsoSDiZ6vq709X-273"> <mxGeometry relative="1" as="geometry"> <Array as="points"> <mxPoint x="810" y="400" /> <mxPoint x="810" y="400" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-279" value="failed" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" vertex="1" connectable="0" parent="6FWRLcRsoSDiZ6vq709X-278"> <mxGeometry x="0.2222" y="-3" relative="1" as="geometry"> <mxPoint x="-15" y="-13" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-5" value="MRM_OPERATING" style="swimlane;whiteSpace=wrap;html=1;gradientColor=none;swimlaneFillColor=default;fillColor=default;" vertex="1" parent="WIyWlLk6GJQsqaUBKTNV-1"> <mxGeometry x="310" y="160" width="460" height="300" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-23" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=1;exitY=0.5;exitDx=0;exitDy=0;entryX=0;entryY=0.5;entryDx=0;entryDy=0;" edge="1" parent="6FWRLcRsoSDiZ6vq709X-5" source="6FWRLcRsoSDiZ6vq709X-12" target="6FWRLcRsoSDiZ6vq709X-14"> <mxGeometry relative="1" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-31" value="pull over unavailable&lt;br&gt;comfortable_stop available (ca)" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" vertex="1" connectable="0" parent="6FWRLcRsoSDiZ6vq709X-23"> <mxGeometry x="-0.12" y="-1" relative="1" as="geometry"> <mxPoint x="9" y="-24" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-24" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;entryX=0;entryY=0.5;entryDx=0;entryDy=0;" edge="1" parent="6FWRLcRsoSDiZ6vq709X-5" source="6FWRLcRsoSDiZ6vq709X-12" target="6FWRLcRsoSDiZ6vq709X-13"> <mxGeometry relative="1" as="geometry"> <Array as="points"> <mxPoint x="35" y="80" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-26" value="pull over available (pa)" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" vertex="1" connectable="0" parent="6FWRLcRsoSDiZ6vq709X-24"> <mxGeometry x="0.1529" y="-1" relative="1" as="geometry"> <mxPoint x="13" y="-11" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-25" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" edge="1" parent="6FWRLcRsoSDiZ6vq709X-5" source="6FWRLcRsoSDiZ6vq709X-12" target="6FWRLcRsoSDiZ6vq709X-17"> <mxGeometry relative="1" as="geometry"> <Array as="points"> <mxPoint x="35" y="240" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-33" value="pull over unavailable&lt;br style=&quot;border-color: var(--border-color);&quot;&gt;comfortable_stop unavailable&lt;br&gt;emergency_stop available (ea)" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" vertex="1" connectable="0" parent="6FWRLcRsoSDiZ6vq709X-25"> <mxGeometry x="0.3217" relative="1" as="geometry"> <mxPoint x="-4" y="-26" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-12" value="" style="ellipse;whiteSpace=wrap;html=1;fillColor=#000000;" vertex="1" parent="6FWRLcRsoSDiZ6vq709X-5"> <mxGeometry x="20" y="143.75" width="30" height="30" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-27" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=0.5;exitY=1;exitDx=0;exitDy=0;" edge="1" parent="6FWRLcRsoSDiZ6vq709X-5" source="6FWRLcRsoSDiZ6vq709X-13" target="6FWRLcRsoSDiZ6vq709X-14"> <mxGeometry relative="1" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-30" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" edge="1" parent="6FWRLcRsoSDiZ6vq709X-5" source="6FWRLcRsoSDiZ6vq709X-13" target="6FWRLcRsoSDiZ6vq709X-17"> <mxGeometry relative="1" as="geometry"> <Array as="points"> <mxPoint x="420" y="80" /> <mxPoint x="420" y="240" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-13" value="pull_over" style="rounded=1;whiteSpace=wrap;html=1;" vertex="1" parent="6FWRLcRsoSDiZ6vq709X-5"> <mxGeometry x="240" y="60" width="150" height="40" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-28" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=0.5;exitY=1;exitDx=0;exitDy=0;" edge="1" parent="6FWRLcRsoSDiZ6vq709X-5" source="6FWRLcRsoSDiZ6vq709X-14" target="6FWRLcRsoSDiZ6vq709X-17"> <mxGeometry relative="1" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-152" value="ea" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" vertex="1" connectable="0" parent="6FWRLcRsoSDiZ6vq709X-28"> <mxGeometry x="-0.1294" y="-1" relative="1" as="geometry"> <mxPoint x="9" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-156" value="ea" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" vertex="1" connectable="0" parent="6FWRLcRsoSDiZ6vq709X-28"> <mxGeometry x="-0.1294" y="-1" relative="1" as="geometry"> <mxPoint x="116" y="-36" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-15" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=0.5;exitY=1;exitDx=0;exitDy=0;" edge="1" parent="6FWRLcRsoSDiZ6vq709X-5" source="6FWRLcRsoSDiZ6vq709X-13" target="6FWRLcRsoSDiZ6vq709X-13"> <mxGeometry relative="1" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-17" value="emergency_stop" style="rounded=1;whiteSpace=wrap;html=1;" vertex="1" parent="6FWRLcRsoSDiZ6vq709X-5"> <mxGeometry x="240" y="220" width="150" height="37.5" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-32" value="" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" edge="1" parent="6FWRLcRsoSDiZ6vq709X-5" source="6FWRLcRsoSDiZ6vq709X-13" target="6FWRLcRsoSDiZ6vq709X-14"> <mxGeometry relative="1" as="geometry"> <mxPoint x="585" y="260" as="sourcePoint" /> <mxPoint x="585" y="380" as="targetPoint" /> <Array as="points" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-150" value="ca" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" vertex="1" connectable="0" parent="6FWRLcRsoSDiZ6vq709X-32"> <mxGeometry x="-0.1" y="1" relative="1" as="geometry"> <mxPoint x="7" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-14" value="comfortable_stop" style="rounded=1;whiteSpace=wrap;html=1;" vertex="1" parent="6FWRLcRsoSDiZ6vq709X-5"> <mxGeometry x="240" y="140" width="150" height="37.5" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-18" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=0;exitY=0.5;exitDx=0;exitDy=0;entryX=0;entryY=1;entryDx=0;entryDy=0;" edge="1" parent="WIyWlLk6GJQsqaUBKTNV-1" source="6FWRLcRsoSDiZ6vq709X-2" target="6FWRLcRsoSDiZ6vq709X-2"> <mxGeometry relative="1" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-276" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" edge="1" parent="WIyWlLk6GJQsqaUBKTNV-1" source="6FWRLcRsoSDiZ6vq709X-272" target="6FWRLcRsoSDiZ6vq709X-5"> <mxGeometry relative="1" as="geometry"> <Array as="points"> <mxPoint x="780" y="220" /> <mxPoint x="780" y="220" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-277" value="pa" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" vertex="1" connectable="0" parent="6FWRLcRsoSDiZ6vq709X-276"> <mxGeometry x="-0.1556" y="-2" relative="1" as="geometry"> <mxPoint x="-6" y="-8" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-272" value="&lt;b&gt;MRM_SUCCEEDED&lt;/b&gt;" style="rounded=0;whiteSpace=wrap;html=1;" vertex="1" parent="WIyWlLk6GJQsqaUBKTNV-1"> <mxGeometry x="850" y="200" width="130" height="80" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-273" value="&lt;b&gt;MRM_FAILED&lt;/b&gt;" style="rounded=0;whiteSpace=wrap;html=1;" vertex="1" parent="WIyWlLk6GJQsqaUBKTNV-1"> <mxGeometry x="850" y="350" width="130" height="80" as="geometry" /> </mxCell> </root> </mxGraphModel> </diagram> </mxfile> " + content="<mxfile host="app.diagrams.net" modified="2024-03-01T14:04:38.320Z" agent="Mozilla/5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) Chrome/122.0.0.0 Safari/537.36" etag="jomwCx6bxG3LysB-HIk4" version="23.1.4" type="device"> <diagram id="C5RBs43oDa-KdzZeNtuy" name="Page-1"> <mxGraphModel dx="1364" dy="759" grid="1" gridSize="10" guides="1" tooltips="1" connect="1" arrows="1" fold="1" page="1" pageScale="1" pageWidth="1169" pageHeight="827" math="0" shadow="0"> <root> <mxCell id="WIyWlLk6GJQsqaUBKTNV-0" /> <mxCell id="WIyWlLk6GJQsqaUBKTNV-1" parent="WIyWlLk6GJQsqaUBKTNV-0" /> <mxCell id="6FWRLcRsoSDiZ6vq709X-281" value="" style="rounded=0;whiteSpace=wrap;html=1;" parent="WIyWlLk6GJQsqaUBKTNV-1" vertex="1"> <mxGeometry x="280" y="140" width="720" height="340" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-5" value="MRM_OPERATING" style="swimlane;whiteSpace=wrap;html=1;gradientColor=none;swimlaneFillColor=default;fillColor=default;" parent="WIyWlLk6GJQsqaUBKTNV-1" vertex="1"> <mxGeometry x="310" y="160" width="460" height="300" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-24" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" parent="6FWRLcRsoSDiZ6vq709X-5" source="6FWRLcRsoSDiZ6vq709X-12" target="6FWRLcRsoSDiZ6vq709X-13" edge="1"> <mxGeometry relative="1" as="geometry"> <Array as="points"> <mxPoint x="35" y="80" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-26" value="pull over available (pa)" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" parent="6FWRLcRsoSDiZ6vq709X-24" vertex="1" connectable="0"> <mxGeometry x="0.1529" y="-1" relative="1" as="geometry"> <mxPoint x="13" y="-11" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-25" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" parent="6FWRLcRsoSDiZ6vq709X-5" source="6FWRLcRsoSDiZ6vq709X-12" target="6FWRLcRsoSDiZ6vq709X-17" edge="1"> <mxGeometry relative="1" as="geometry"> <Array as="points"> <mxPoint x="35" y="250" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-33" value="pull over unavailable&lt;br style=&quot;border-color: var(--border-color);&quot;&gt;comfortable_stop unavailable&lt;br&gt;emergency_stop available (ea)" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" parent="6FWRLcRsoSDiZ6vq709X-25" vertex="1" connectable="0"> <mxGeometry x="0.3217" relative="1" as="geometry"> <mxPoint x="-4" y="-26" as="offset" /> </mxGeometry> </mxCell> <mxCell id="0iophzwyXz64g-DfP8aO-1" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" edge="1" parent="6FWRLcRsoSDiZ6vq709X-5" source="6FWRLcRsoSDiZ6vq709X-12" target="6FWRLcRsoSDiZ6vq709X-14"> <mxGeometry relative="1" as="geometry" /> </mxCell> <mxCell id="0iophzwyXz64g-DfP8aO-2" value="pull over unavailable&lt;br style=&quot;border-color: var(--border-color);&quot;&gt;comfortable_stop available (ca)" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" vertex="1" connectable="0" parent="0iophzwyXz64g-DfP8aO-1"> <mxGeometry x="-0.1263" y="-4" relative="1" as="geometry"> <mxPoint x="7" y="-24" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-12" value="" style="ellipse;whiteSpace=wrap;html=1;fillColor=#000000;" parent="6FWRLcRsoSDiZ6vq709X-5" vertex="1"> <mxGeometry x="20" y="145" width="30" height="30" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-30" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" parent="6FWRLcRsoSDiZ6vq709X-5" source="6FWRLcRsoSDiZ6vq709X-13" target="6FWRLcRsoSDiZ6vq709X-17" edge="1"> <mxGeometry relative="1" as="geometry"> <Array as="points"> <mxPoint x="420" y="80" /> <mxPoint x="420" y="240" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-13" value="pull_over" style="rounded=1;whiteSpace=wrap;html=1;" parent="6FWRLcRsoSDiZ6vq709X-5" vertex="1"> <mxGeometry x="240" y="40" width="150" height="60" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-28" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" parent="6FWRLcRsoSDiZ6vq709X-5" source="6FWRLcRsoSDiZ6vq709X-14" target="6FWRLcRsoSDiZ6vq709X-17" edge="1"> <mxGeometry relative="1" as="geometry"> <Array as="points"> <mxPoint x="290" y="200" /> <mxPoint x="290" y="200" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-152" value="ea || failed" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" parent="6FWRLcRsoSDiZ6vq709X-28" vertex="1" connectable="0"> <mxGeometry x="-0.1294" y="-1" relative="1" as="geometry"> <mxPoint x="31" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-156" value="ea || failed" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" parent="6FWRLcRsoSDiZ6vq709X-28" vertex="1" connectable="0"> <mxGeometry x="-0.1294" y="-1" relative="1" as="geometry"> <mxPoint x="131" y="-36" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-15" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=0.5;exitY=1;exitDx=0;exitDy=0;" parent="6FWRLcRsoSDiZ6vq709X-5" source="6FWRLcRsoSDiZ6vq709X-13" target="6FWRLcRsoSDiZ6vq709X-13" edge="1"> <mxGeometry relative="1" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-17" value="emergency_stop" style="rounded=1;whiteSpace=wrap;html=1;" parent="6FWRLcRsoSDiZ6vq709X-5" vertex="1"> <mxGeometry x="240" y="220" width="150" height="60" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-32" value="" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" parent="6FWRLcRsoSDiZ6vq709X-5" source="6FWRLcRsoSDiZ6vq709X-13" target="6FWRLcRsoSDiZ6vq709X-14" edge="1"> <mxGeometry relative="1" as="geometry"> <mxPoint x="585" y="260" as="sourcePoint" /> <mxPoint x="585" y="380" as="targetPoint" /> <Array as="points"> <mxPoint x="290" y="110" /> <mxPoint x="290" y="110" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-150" value="ca" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" parent="6FWRLcRsoSDiZ6vq709X-32" vertex="1" connectable="0"> <mxGeometry x="-0.1" y="1" relative="1" as="geometry"> <mxPoint x="7" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-14" value="comfortable_stop" style="rounded=1;whiteSpace=wrap;html=1;" parent="6FWRLcRsoSDiZ6vq709X-5" vertex="1"> <mxGeometry x="240" y="130" width="150" height="60" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-284" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" parent="WIyWlLk6GJQsqaUBKTNV-1" source="6FWRLcRsoSDiZ6vq709X-281" target="6FWRLcRsoSDiZ6vq709X-3" edge="1"> <mxGeometry relative="1" as="geometry"> <Array as="points"> <mxPoint x="230" y="360" /> <mxPoint x="230" y="360" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-285" value="not emergency" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" parent="6FWRLcRsoSDiZ6vq709X-284" vertex="1" connectable="0"> <mxGeometry x="0.02" relative="1" as="geometry"> <mxPoint x="1" y="-10" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-4" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=1;exitY=0.5;exitDx=0;exitDy=0;entryX=0;entryY=0.5;entryDx=0;entryDy=0;" parent="WIyWlLk6GJQsqaUBKTNV-1" source="6FWRLcRsoSDiZ6vq709X-2" target="6FWRLcRsoSDiZ6vq709X-3" edge="1"> <mxGeometry relative="1" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-2" value="" style="ellipse;whiteSpace=wrap;html=1;fillColor=#000000;" parent="WIyWlLk6GJQsqaUBKTNV-1" vertex="1"> <mxGeometry x="50" y="305" width="30" height="30" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-19" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=1;exitY=0.25;exitDx=0;exitDy=0;" parent="WIyWlLk6GJQsqaUBKTNV-1" source="6FWRLcRsoSDiZ6vq709X-3" target="6FWRLcRsoSDiZ6vq709X-281" edge="1"> <mxGeometry relative="1" as="geometry"> <mxPoint x="270" y="290" as="targetPoint" /> <Array as="points"> <mxPoint x="260" y="290" /> <mxPoint x="260" y="290" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-21" value="emergency" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" parent="6FWRLcRsoSDiZ6vq709X-19" vertex="1" connectable="0"> <mxGeometry x="-0.0719" y="-3" relative="1" as="geometry"> <mxPoint y="-13" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-3" value="&lt;b&gt;NORMAL&lt;/b&gt;" style="rounded=0;whiteSpace=wrap;html=1;" parent="WIyWlLk6GJQsqaUBKTNV-1" vertex="1"> <mxGeometry x="120" y="260" width="60" height="120" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-274" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" parent="WIyWlLk6GJQsqaUBKTNV-1" source="6FWRLcRsoSDiZ6vq709X-5" target="6FWRLcRsoSDiZ6vq709X-272" edge="1"> <mxGeometry relative="1" as="geometry"> <Array as="points"> <mxPoint x="810" y="260" /> <mxPoint x="810" y="260" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-275" value="succeeded" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" parent="6FWRLcRsoSDiZ6vq709X-274" vertex="1" connectable="0"> <mxGeometry x="-0.0667" y="-3" relative="1" as="geometry"> <mxPoint y="-13" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-278" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" parent="WIyWlLk6GJQsqaUBKTNV-1" source="6FWRLcRsoSDiZ6vq709X-17" target="6FWRLcRsoSDiZ6vq709X-273" edge="1"> <mxGeometry relative="1" as="geometry"> <Array as="points"> <mxPoint x="740" y="414" /> <mxPoint x="740" y="414" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-279" value="failed" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" parent="6FWRLcRsoSDiZ6vq709X-278" vertex="1" connectable="0"> <mxGeometry x="0.2222" y="-3" relative="1" as="geometry"> <mxPoint x="8" y="-13" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-18" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=0;exitY=0.5;exitDx=0;exitDy=0;entryX=0;entryY=1;entryDx=0;entryDy=0;" parent="WIyWlLk6GJQsqaUBKTNV-1" source="6FWRLcRsoSDiZ6vq709X-2" target="6FWRLcRsoSDiZ6vq709X-2" edge="1"> <mxGeometry relative="1" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-276" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;" parent="WIyWlLk6GJQsqaUBKTNV-1" source="6FWRLcRsoSDiZ6vq709X-272" target="6FWRLcRsoSDiZ6vq709X-13" edge="1"> <mxGeometry relative="1" as="geometry"> <Array as="points"> <mxPoint x="780" y="220" /> <mxPoint x="780" y="220" /> </Array> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-277" value="pa" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" parent="6FWRLcRsoSDiZ6vq709X-276" vertex="1" connectable="0"> <mxGeometry x="-0.1556" y="-2" relative="1" as="geometry"> <mxPoint x="13" y="-8" as="offset" /> </mxGeometry> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-272" value="&lt;b&gt;MRM_SUCCEEDED&lt;/b&gt;" style="rounded=0;whiteSpace=wrap;html=1;" parent="WIyWlLk6GJQsqaUBKTNV-1" vertex="1"> <mxGeometry x="850" y="200" width="130" height="80" as="geometry" /> </mxCell> <mxCell id="6FWRLcRsoSDiZ6vq709X-273" value="&lt;b&gt;MRM_FAILED&lt;/b&gt;" style="rounded=0;whiteSpace=wrap;html=1;" parent="WIyWlLk6GJQsqaUBKTNV-1" vertex="1"> <mxGeometry x="850" y="370" width="130" height="80" as="geometry" /> </mxCell> </root> </mxGraphModel> </diagram> </mxfile> " > - - + + + + -
-
+
+
- not emergency + MRM_OPERATING
- not emergency + MRM_OPERATING - - - - - - + + -
+
- emergency -
-
-
- - emergency - - - - - - -
-
-
- NORMAL + pull over available (pa)
- NORMAL + pull over available (pa)
- - + + -
+
- succeeded + pull over unavailable +
+ comfortable_stop unavailable +
+ emergency_stop available (ea)
- succeeded + pull over unavailable... - - + + -
+
- failed + pull over unavailable +
+ comfortable_stop available (ca)
- failed + pull over unavailable... - - - + + + +
-
- MRM_OPERATING +
+ pull_over
- MRM_OPERATING + pull_over - - + + -
+
- pull over unavailable -
- comfortable_stop available (ca) + ea || failed
- pull over unavailable... + ea || failed - - -
+
- pull over available (pa) + ea || failed
- pull over available (pa) + ea || failed + + + + + + +
+
+
+ emergency_stop +
+
+
+
+ emergency_stop
- - + + -
+
- pull over unavailable -
- comfortable_stop unavailable -
- emergency_stop available (ea) + ca
- pull over unavailable... + ca - - - - - - +
- pull_over + comfortable_stop
- pull_over + comfortable_stop
- - + + -
+
- ea + not emergency
- ea + not emergency + + + + + -
+
- ea + emergency
- ea + emergency - + -
+
- emergency_stop + NORMAL
- emergency_stop + NORMAL - - + + -
+
- ca + succeeded
- ca + succeeded - + + -
-
-
- comfortable_stop +
+
+
+ failed
- comfortable_stop + failed - - + + -
+
- pa + pa @@ -334,13 +332,13 @@ MRM_SUCCEEDED - +
@@ -349,7 +347,7 @@
- MRM_FAILED + MRM_FAILED diff --git a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp index 98f12ba6b9467..1dd0f6b081337 100644 --- a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp +++ b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp @@ -47,6 +47,8 @@ struct Param { int update_rate; double timeout_operation_mode_availability; + double timeout_call_mrm_behavior; + double timeout_cancel_mrm_behavior; bool use_emergency_holding; double timeout_emergency_recovery; bool use_parking_after_stopped; @@ -61,6 +63,9 @@ class MrmHandler : public rclcpp::Node MrmHandler(); private: + // type + enum RequestType { CALL, CANCEL }; + // Subscribers rclcpp::Subscription::SharedPtr sub_odom_; rclcpp::Subscription::SharedPtr @@ -120,10 +125,9 @@ class MrmHandler : public rclcpp::Node rclcpp::CallbackGroup::SharedPtr client_mrm_emergency_stop_group_; rclcpp::Client::SharedPtr client_mrm_emergency_stop_; - void callMrmBehavior( - const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior) const; - void cancelMrmBehavior( - const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior) const; + bool requestMrmBehavior( + const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior, + RequestType request_type) const; void logMrmCallingResult( const tier4_system_msgs::srv::OperateMrm::Response & result, const std::string & behavior, bool is_call) const; @@ -148,6 +152,7 @@ class MrmHandler : public rclcpp::Node void transitionTo(const int new_state); void updateMrmState(); void operateMrm(); + void handleFailedRequest(); autoware_adapi_v1_msgs::msg::MrmState::_behavior_type getCurrentMrmBehavior(); bool isStopped(); bool isEmergency() const; diff --git a/system/mrm_handler/schema/mrm_handler.schema.json b/system/mrm_handler/schema/mrm_handler.schema.json index aedb8076ef05c..9a50c6a326f01 100644 --- a/system/mrm_handler/schema/mrm_handler.schema.json +++ b/system/mrm_handler/schema/mrm_handler.schema.json @@ -16,6 +16,16 @@ "description": "If the input `operation_mode_availability` topic cannot be received for more than `timeout_operation_mode_availability`, vehicle will make an emergency stop.", "default": 0.5 }, + "timeout_call_mrm_behavior": { + "type": "number", + "description": "If a service is requested to the mrm operator and the response is not returned by `timeout_call_mrm_behavior`, the timeout occurs.", + "default": 0.01 + }, + "timeout_cancel_mrm_behavior": { + "type": "number", + "description": "If a service is requested to the mrm operator and the response is not returned by `timeout_cancel_mrm_behavior`, the timeout occurs.", + "default": 0.01 + }, "use_emergency_holding": { "type": "boolean", "description": "If this parameter is true, the handler does not recover to the NORMAL state.", @@ -56,6 +66,8 @@ "required": [ "update_rate", "timeout_operation_mode_availability", + "timeout_call_mrm_behavior", + "timeout_cancel_mrm_behavior", "use_emergency_holding", "timeout_emergency_recovery", "use_parking_after_stopped", diff --git a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp index 03cc08de2d160..e8e692f755e2d 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -24,6 +24,9 @@ MrmHandler::MrmHandler() : Node("mrm_handler") param_.update_rate = declare_parameter("update_rate", 10); param_.timeout_operation_mode_availability = declare_parameter("timeout_operation_mode_availability", 0.5); + param_.timeout_call_mrm_behavior = declare_parameter("timeout_call_mrm_behavior", 0.01); + param_.timeout_cancel_mrm_behavior = + declare_parameter("timeout_cancel_mrm_behavior", 0.01); param_.use_emergency_holding = declare_parameter("use_emergency_holding", false); param_.timeout_emergency_recovery = declare_parameter("timeout_emergency_recovery", 5.0); param_.use_parking_after_stopped = declare_parameter("use_parking_after_stopped", false); @@ -210,18 +213,27 @@ void MrmHandler::operateMrm() if (mrm_state_.state == MrmState::NORMAL) { // Cancel MRM behavior when returning to NORMAL state const auto current_mrm_behavior = MrmState::NONE; - if (current_mrm_behavior != mrm_state_.behavior) { - cancelMrmBehavior(mrm_state_.behavior); + if (current_mrm_behavior == mrm_state_.behavior) { + return; + } + if (requestMrmBehavior(mrm_state_.behavior, RequestType::CANCEL)) { mrm_state_.behavior = current_mrm_behavior; + } else { + handleFailedRequest(); } return; } if (mrm_state_.state == MrmState::MRM_OPERATING) { const auto current_mrm_behavior = getCurrentMrmBehavior(); - if (current_mrm_behavior != mrm_state_.behavior) { - cancelMrmBehavior(mrm_state_.behavior); - callMrmBehavior(current_mrm_behavior); + if (current_mrm_behavior == mrm_state_.behavior) { + return; + } + if (!requestMrmBehavior(mrm_state_.behavior, RequestType::CANCEL)) { + handleFailedRequest(); + } else if (requestMrmBehavior(current_mrm_behavior, RequestType::CALL)) { mrm_state_.behavior = current_mrm_behavior; + } else { + handleFailedRequest(); } return; } @@ -237,88 +249,94 @@ void MrmHandler::operateMrm() RCLCPP_WARN(this->get_logger(), "invalid MRM state: %d", mrm_state_.state); } -void MrmHandler::callMrmBehavior( - const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior) const +void MrmHandler::handleFailedRequest() { using autoware_adapi_v1_msgs::msg::MrmState; - auto request = std::make_shared(); - request->operate = true; - - if (mrm_behavior == MrmState::NONE) { - RCLCPP_WARN(this->get_logger(), "MRM behavior is None. Do nothing."); - return; - } - if (mrm_behavior == MrmState::PULL_OVER) { - auto result = client_mrm_pull_over_->async_send_request(request).get(); - if (result->response.success == true) { - RCLCPP_WARN(this->get_logger(), "Pull over is operated"); - } else { - RCLCPP_ERROR(this->get_logger(), "Pull over failed to operate"); - } - return; - } - if (mrm_behavior == MrmState::COMFORTABLE_STOP) { - auto result = client_mrm_comfortable_stop_->async_send_request(request).get(); - if (result->response.success == true) { - RCLCPP_WARN(this->get_logger(), "Comfortable stop is operated"); - } else { - RCLCPP_ERROR(this->get_logger(), "Comfortable stop failed to operate"); - } - return; - } - if (mrm_behavior == MrmState::EMERGENCY_STOP) { - auto result = client_mrm_emergency_stop_->async_send_request(request).get(); - if (result->response.success == true) { - RCLCPP_WARN(this->get_logger(), "Emergency stop is operated"); - } else { - RCLCPP_ERROR(this->get_logger(), "Emergency stop failed to operate"); - } - return; + if (requestMrmBehavior(MrmState::EMERGENCY_STOP, CALL)) { + if (mrm_state_.state != MrmState::MRM_OPERATING) transitionTo(MrmState::MRM_OPERATING); + } else { + transitionTo(MrmState::MRM_FAILED); } - RCLCPP_WARN(this->get_logger(), "invalid MRM behavior: %d", mrm_behavior); + mrm_state_.behavior = MrmState::EMERGENCY_STOP; } -void MrmHandler::cancelMrmBehavior( - const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior) const +bool MrmHandler::requestMrmBehavior( + const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior, + RequestType request_type) const { using autoware_adapi_v1_msgs::msg::MrmState; auto request = std::make_shared(); - request->operate = false; - - if (mrm_behavior == MrmState::NONE) { - // Do nothing - return; + if (request_type == RequestType::CALL) { + request->operate = true; + } else if (request_type == RequestType::CANCEL) { + request->operate = false; + } else { + RCLCPP_ERROR(this->get_logger(), "invalid request type: %d", request_type); + return false; } - if (mrm_behavior == MrmState::PULL_OVER) { - auto result = client_mrm_pull_over_->async_send_request(request).get(); - if (result->response.success == true) { - RCLCPP_WARN(this->get_logger(), "Pull over is canceled"); - } else { - RCLCPP_ERROR(this->get_logger(), "Pull over failed to cancel"); + const auto duration = std::chrono::duration>( + request->operate ? param_.timeout_call_mrm_behavior : param_.timeout_cancel_mrm_behavior); + std::shared_future> future; + + const auto behavior2string = [](const int behavior) { + if (behavior == MrmState::NONE) { + return "NONE"; } - return; - } - if (mrm_behavior == MrmState::COMFORTABLE_STOP) { - auto result = client_mrm_comfortable_stop_->async_send_request(request).get(); - if (result->response.success == true) { - RCLCPP_WARN(this->get_logger(), "Comfortable stop is canceled"); - } else { - RCLCPP_ERROR(this->get_logger(), "Comfortable stop failed to cancel"); + if (behavior == MrmState::PULL_OVER) { + return "PULL_OVER"; } - return; + if (behavior == MrmState::COMFORTABLE_STOP) { + return "COMFORTABLE_STOP"; + } + if (behavior == MrmState::EMERGENCY_STOP) { + return "EMERGENCY_STOP"; + } + const auto msg = "invalid behavior: " + std::to_string(behavior); + throw std::runtime_error(msg); + }; + + switch (mrm_behavior) { + case MrmState::NONE: + RCLCPP_WARN(this->get_logger(), "MRM behavior is None. Do nothing."); + return true; + case MrmState::PULL_OVER: { + future = client_mrm_pull_over_->async_send_request(request).future.share(); + break; + } + case MrmState::COMFORTABLE_STOP: { + future = client_mrm_comfortable_stop_->async_send_request(request).future.share(); + break; + } + case MrmState::EMERGENCY_STOP: { + future = client_mrm_emergency_stop_->async_send_request(request).future.share(); + break; + } + default: + RCLCPP_ERROR(this->get_logger(), "invalid behavior: %d", mrm_behavior); + return false; } - if (mrm_behavior == MrmState::EMERGENCY_STOP) { - auto result = client_mrm_emergency_stop_->async_send_request(request).get(); + + if (future.wait_for(duration) == std::future_status::ready) { + const auto result = future.get(); if (result->response.success == true) { - RCLCPP_WARN(this->get_logger(), "Emergency stop is canceled"); + RCLCPP_WARN( + this->get_logger(), request->operate ? "%s is operated." : "%s is canceled.", + behavior2string(mrm_behavior)); + return true; } else { - RCLCPP_ERROR(this->get_logger(), "Emergency stop failed to cancel"); + RCLCPP_ERROR( + this->get_logger(), request->operate ? "%s failed to operate." : "%s failed to cancel.", + behavior2string(mrm_behavior)); + return false; } - return; + } else { + RCLCPP_ERROR( + this->get_logger(), request->operate ? "%s call timed out." : "%s cancel timed out.", + behavior2string(mrm_behavior)); + return false; } - RCLCPP_WARN(this->get_logger(), "invalid MRM behavior: %d", mrm_behavior); } bool MrmHandler::isDataReady()