Skip to content

Commit 6505176

Browse files
ste93ste93
committed
updates after trial in simulation
Signed-off-by: Stefano Bernagozzi <[email protected]>
1 parent ae02afa commit 6505176

File tree

5 files changed

+82
-23
lines changed

5 files changed

+82
-23
lines changed

‎src/behavior_tree/BT/bt_scheduler.xml‎

Lines changed: 10 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -14,37 +14,37 @@
1414

1515
<Sequence>
1616
<Inverter>
17-
<Condition ID="ROS2Condition" interface="ROS2SERVICE" isMonitored="false" name="IsPoiDone3"/>
17+
<Condition ID="ROS2Condition" interface="ROS2SERVICE" isMonitored="false" name="IsPoiDone2"/>
1818
</Inverter>
19-
<Action ID="ROS2Action" interface="ROS2SERVICE" isMonitored="false" name="SetPoi3"/>
19+
<Action ID="ROS2Action" interface="ROS2SERVICE" isMonitored="false" name="SetPoi2"/>
2020
</Sequence>
2121

2222
<Sequence>
2323
<Inverter>
24-
<Condition ID="ROS2Condition" interface="ROS2SERVICE" isMonitored="false" name="IsPoiDone4"/>
24+
<Condition ID="ROS2Condition" interface="ROS2SERVICE" isMonitored="false" name="IsPoiDone3"/>
2525
</Inverter>
26-
<Action ID="ROS2Action" interface="ROS2SERVICE" isMonitored="false" name="SetPoi4"/>
26+
<Action ID="ROS2Action" interface="ROS2SERVICE" isMonitored="false" name="SetPoi3"/>
2727
</Sequence>
2828

2929
<Sequence>
3030
<Inverter>
31-
<Condition ID="ROS2Condition" interface="ROS2SERVICE" isMonitored="false" name="IsPoiDone6"/>
31+
<Condition ID="ROS2Condition" interface="ROS2SERVICE" isMonitored="false" name="IsPoiDone5"/>
3232
</Inverter>
33-
<Action ID="ROS2Action" interface="ROS2SERVICE" isMonitored="false" name="SetPoi6"/>
33+
<Action ID="ROS2Action" interface="ROS2SERVICE" isMonitored="false" name="SetPoi5"/>
3434
</Sequence>
3535

3636
<Sequence>
3737
<Inverter>
38-
<Condition ID="ROS2Condition" interface="ROS2SERVICE" isMonitored="false" name="IsPoiDone9"/>
38+
<Condition ID="ROS2Condition" interface="ROS2SERVICE" isMonitored="false" name="IsPoiDone8"/>
3939
</Inverter>
40-
<Action ID="ROS2Action" interface="ROS2SERVICE" isMonitored="false" name="SetPoi9"/>
40+
<Action ID="ROS2Action" interface="ROS2SERVICE" isMonitored="false" name="SetPoi8"/>
4141
</Sequence>
4242

4343
<Sequence>
4444
<Inverter>
45-
<Condition ID="ROS2Condition" interface="ROS2SERVICE" isMonitored="false" name="IsPoiDone11"/>
45+
<Condition ID="ROS2Condition" interface="ROS2SERVICE" isMonitored="false" name="IsPoiDone10"/>
4646
</Inverter>
47-
<Action ID="ROS2Action" interface="ROS2SERVICE" isMonitored="false" name="SetPoi11"/>
47+
<Action ID="ROS2Action" interface="ROS2SERVICE" isMonitored="false" name="SetPoi10"/>
4848
</Sequence>
4949

5050
<Action ID="ROS2Action" interface="ROS2SERVICE" isMonitored="false" name="ResetTourAndFlags"/>

‎src/components/planner_component/planner_component/PlannerComponent.py‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -156,6 +156,7 @@ def execute_callback(self, goal_handle):
156156
except Exception as e:
157157
self.get_logger().error(f'Error while succeeding goal: {e}')
158158

159+
self.get_logger().info(f"sequence_of_pois: {plan}")
159160
result = Plan.Result()
160161
result.is_ok = True
161162
# result.sequence = feedback_msg.partial_sequence

‎src/components/scheduler_component/src/SchedulerComponent.cpp‎

Lines changed: 6 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -188,8 +188,13 @@ void SchedulerComponent::GetCurrentPoiForNavigation([[maybe_unused]] const std::
188188
{
189189
response->poi_name = m_tourStorage->GetTour().getPoIsList()[m_currentPoi] + "_2";
190190
}
191+
int poi_number_for_navigation = m_currentPoi * 2;
192+
if (!m_alternative_poi)
193+
{
194+
poi_number_for_navigation -= 1;
195+
}
196+
response->poi_number = poi_number_for_navigation;
191197
RCLCPP_INFO(m_node->get_logger(), "SchedulerComponent::GetCurrentPoiForNavigation name: %s", response->poi_name.c_str());
192-
response->poi_number = m_currentPoi;
193198
response->is_ok = true;
194199
}
195200

‎src/skills/go_to_poi_action_skill/src/GoToPoiActionSkillSM.scxml‎

Lines changed: 48 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,7 @@
44
<data id="poi_name" expr="&apos;&apos;"/>
55
<data id="status" expr="0"/>
66
<data id="result" expr="true"/>
7+
<data id="goal_result" expr="true"/>
78
<data id="SKILL_SUCCESS" expr="0"/>
89
<data id="SKILL_FAILURE" expr="1"/>
910
<data id="SKILL_RUNNING" expr="2"/>
@@ -12,6 +13,9 @@
1213
<!-- GoToPoiActionSkill is a client for GetCurrentPoi from the SchedulerComponent -->
1314
<!-- GoToPoiActionSkill is a client for GoToPoi action from the Navigation Component -->
1415
<state id="idle">
16+
<onentry>
17+
<log expr="'idle'"/>
18+
</onentry>
1519
<transition target="getPoi" event="CMD_TICK">
1620
<send event="SchedulerComponent.GetCurrentPoiForNavigation.Call"/>
1721
</transition>
@@ -22,6 +26,9 @@
2226
</transition>
2327
</state>
2428
<state id="getPoi">
29+
<onentry>
30+
<log expr="'getPoi'"/>
31+
</onentry>
2532
<transition target="goalResponse" event="SchedulerComponent.GetCurrentPoiForNavigation.Return">
2633
<assign location="poi_number" expr="_event.data.poi_number"/>
2734
<assign location="poi_name" expr="_event.data.poi_name"/>
@@ -36,13 +43,19 @@
3643
<transition target="getCurrentPoiResponseHalt" event="CMD_HALT"/>
3744
</state>
3845
<state id="getCurrentPoiResponseHalt">
46+
<onentry>
47+
<log expr="'getCurrentPoiResponseHalt'"/>
48+
</onentry>
3949
<transition target="idle" event="SchedulerComponent.GetCurrentPoiForNavigation.Return">
4050
<send event="HALT_RESPONSE">
4151
<param name="is_ok" expr="true"/>
4252
</send>
4353
</transition>
4454
</state>
4555
<state id="goalResponse">
56+
<onentry>
57+
<log expr="'goalResponse'"/>
58+
</onentry>
4659
<transition event="NavigationComponent.GoToPoi.GoalResponse" cond="_event.data.call_succeeded" target="wait_result"/>
4760
<transition event="NavigationComponent.GoToPoi.GoalResponse" cond="_event.data.call_succeeded == false" target="send_failure"/>
4861
<transition target="goalResponse" event="CMD_TICK">
@@ -58,6 +71,9 @@
5871
</transition>
5972
</state>
6073
<state id="wait_result">
74+
<onentry>
75+
<log expr="'wait_result'"/>
76+
</onentry>
6177
<transition target="wait_result" event="CMD_TICK">
6278
<send event="TICK_RESPONSE">
6379
<param name="is_ok" expr="true"/>
@@ -68,7 +84,7 @@
6884
<assign location="status" expr="_event.data.status"/>
6985
</transition>
7086
<transition target="evaluate_result" event="NavigationComponent.GoToPoi.ResultResponse">
71-
<assign location="result" expr="_event.data.is_ok"/>
87+
<assign location="goal_result" expr="_event.data.is_ok"/>
7288
</transition>
7389
<transition target="idle" event="CMD_HALT">
7490
<send event="HALT_RESPONSE">
@@ -77,25 +93,44 @@
7793
</transition>
7894
</state>
7995

80-
<state id="evaluate_result">
81-
<transition cond="result == true" target="goal_done"/>
82-
<transition cond="result == false" target="send_failure"/>
83-
</state>
8496

85-
<state id="goal_done">
86-
<transition target="idle" event="CMD_TICK">
87-
<send event="TICK_RESPONSE">
88-
<param name="is_ok" expr="true"/>
89-
<param name="status" expr="SKILL_SUCCESS"/>
90-
</send>
91-
</transition>
97+
98+
<state id="evaluate_result">
99+
<transition target="goal_done" event="CMD_TICK"/>
92100
<transition target="idle" event="CMD_HALT">
93101
<send event="HALT_RESPONSE">
94102
<param name="is_ok" expr="true"/>
95103
</send>
96104
</transition>
105+
106+
<onentry>
107+
<log expr="'evaluate_result'"/>
108+
109+
</onentry>
110+
</state>
111+
112+
<state id="goal_done">
113+
<onentry>
114+
<log expr="'goal_done'"/>
115+
<if cond="!goal_result">
116+
<send event="TICK_RESPONSE">
117+
<param name="status" expr="SKILL_SUCCESS"/>
118+
<param name="is_ok" expr="true"/>
119+
</send>
120+
<else/>
121+
<send event="TICK_RESPONSE">
122+
<param name="status" expr="SKILL_FAILURE"/>
123+
<param name="is_ok" expr="true"/>
124+
</send>
125+
</if>
126+
127+
</onentry>
128+
<transition target="idle"/>
97129
</state>
98130
<state id="send_failure">
131+
<onentry>
132+
<log expr="'send_failure'"/>
133+
</onentry>
99134
<transition target="idle" event="CMD_TICK">
100135
<send event="TICK_RESPONSE">
101136
<param name="is_ok" expr="true"/>
@@ -109,3 +144,4 @@
109144
</transition>
110145
</state>
111146
</scxml>
147+

‎src/skills/is_at_current_poi_skill/src/IsAtCurrentPoiSkillSM.scxml‎

Lines changed: 17 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -18,25 +18,35 @@
1818
<!-- Condition skill: tick server only -->
1919
<!-- Clients -->
2020
<state id="idle">
21+
<onentry>
22+
<log expr="'idle'"/>
23+
</onentry>
2124
<transition target="getPoi" event="CMD_TICK">
2225
<send event="SchedulerComponent.GetCurrentPoiForNavigation.Call"/>
2326
</transition>
2427
</state>
2528
<state id="getPoi">
29+
<onentry>
30+
<log expr="'getPoi'"/>
31+
</onentry>
2632
<transition target="evaluatePoi" event="SchedulerComponent.GetCurrentPoiForNavigation.Return">
2733
<assign location="m_poi_number" expr="_event.data.poi_number"/>
2834
<assign location="m_poi_name" expr="_event.data.poi_name"/>
2935
<assign location="m_poi_result" expr="true"/>
3036
</transition>
3137
</state>
3238
<state id="evaluatePoi">
39+
<onentry>
40+
<log expr="'evaluatePoi'"/>
41+
</onentry>
3342
<!-- GetCurrentPoi failed -->
3443
<transition cond="!m_poi_result" target="failure"/>
3544
<!-- GetCurrentPoi succeeded, check navigation status -->
3645
<transition target="getStatus"/>
3746
</state>
3847
<state id="getStatus">
3948
<onentry>
49+
<log expr="'getStatus'"/>
4050
<send event="NavigationComponent.GetNavigationStatus.Call"/>
4151
</onentry>
4252
<transition target="evaluateStatus" event="NavigationComponent.GetNavigationStatus.Return">
@@ -45,6 +55,9 @@
4555
</transition>
4656
</state>
4757
<state id="evaluateStatus">
58+
<onentry>
59+
<log expr="'evaluateStatus'"/>
60+
</onentry>
4861
<!-- GetNavigationStatus failed -->
4962
<transition cond="!m_nav_result" target="failure"/>
5063
<!-- Robot is moving, not at POI -->
@@ -54,6 +67,7 @@
5467
</state>
5568
<state id="checkNear">
5669
<onentry>
70+
<log expr="'checkNear'"/>
5771
<send event="NavigationComponent.CheckNearToPoi.Call">
5872
<param name="poi_name" expr="m_poi_name"/>
5973
<param name="distance" expr="distance"/>
@@ -66,6 +80,7 @@
6680
</state>
6781
<state id="finalize">
6882
<onentry>
83+
<log expr="'finalize'"/>
6984
<if cond="m_is_near">
7085
<send event="TICK_RESPONSE">
7186
<param name="status" expr="SKILL_SUCCESS"/>
@@ -82,6 +97,7 @@
8297
</state>
8398
<state id="failure">
8499
<onentry>
100+
<log expr="'failure'"/>
85101
<send event="TICK_RESPONSE">
86102
<param name="status" expr="SKILL_FAILURE"/>
87103
<param name="is_ok" expr="true"/>
@@ -90,3 +106,4 @@
90106
<transition target="idle"/>
91107
</state>
92108
</scxml>
109+

0 commit comments

Comments
 (0)