Skip to content

Commit 3cebe0b

Browse files
MarijaGolubovicMarijaGolubovic
authored
Bug fix and new features (#280)
* Fix blue agresive strategy * Add ignore obstacles in translate * Add parametar in groot * Add new start positions * Add new start positions * Print map resolution anddelete print for table params * Parse color for different start fields * Fix start positions * Add gentle attack * Fix gentle attack * Impruve old startegy with new features * Fix big strategy, replace move stack by other move comand * Fix big blue startegy and add stack * Add post and pri conditions * Fix green startegy * Try fix Client requested to cancel the goal * Fix box driver * Fix green strategy, disable detection on the plate * Fix and test blue strategy, disable detection on the plate * Try implement agresive strategy * Fix agresive strategy * Try impleement big agresive staregy * Fix finish cordinate * Try impleement green strategy * Fix green agresive strategy * Try implement big green agresive straetgy * Fix green side for agresive staretgy * Try implement big blue simple safe strategy(put cake on close plate) * Calibrate small robot * Change start point for small robot * Add ax in small description * Fix small startegy * Fix small staretgy for blue side * Try implement green side for smalll * Add blue_1 start field and implement pick cherry green start * Try implement all small straetgy for green side * Fix homologation strategy * Fix big homologation and safe big blue * Fix safe blue staretgy for big robot * Implement green safe staregy-first part * Implement green safe staregy-first part-fix * Fix strategy * Implement green safe staregy * Try fix staryegy in vs code * Fix staretgy * Fix safe staretgy * Implement green small staretgy for small * Fix safe blue strategy * Fix small staretgy for bouth side and chaneg launch pi for new small start position * Cnegne robot radiius for small 18->25 * Fix green staretgy for big safe startegy * Add base for smal safe startegy * Try to fix blue side * Go backweard when put first set of cakes * Fix both staretgy * Fix strategy, fix lifting lift brown cake * Fix agresive strategy for blue seide, fix position cake taking * Delete points for basket and change cordinates for waiting points * Fix blue agresive strategy * Fix green agresive strategy and add green_a in robot_launch.py * Implement safe straetgy for blue side * Add patern for turbin * Fix small sefe staretgy, add ramp for turbin * Change robot radius for small robot * Fix small waiting time and cordinates for big * Fix safe staretgy for small * Fix safe stareygy for big * Funny action blue * Fix cordinates for funny * Add green funny * Fix funny staretgys * Fix time for strategy * Add cordinatest for small robot in robot_launch.py * Add dispensers for big * Fix dispenser for big * Fix points for big and safe for small * Fix points for agresive * Fix cordinates for dispenser for small safe strategy * Fix finish points small * Fix staregy for smalla agresive
1 parent 8d314ce commit 3cebe0b

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

50 files changed

+5135
-741
lines changed

‎mep3_behavior/include/mep3_behavior/bt_action_node.hpp

+30-6
Original file line numberDiff line numberDiff line change
@@ -102,6 +102,7 @@ class RosActionNode : public BT::ActionNodeBase
102102
void cancelGoal();
103103

104104
protected:
105+
bool should_cancel_goal();
105106

106107
std::shared_ptr<rclcpp::Node> node_;
107108
std::string action_name_;
@@ -335,16 +336,39 @@ template<class T> inline
335336
if (!goal_handle_)
336337
return;
337338

338-
auto future_cancel = action_client_->async_cancel_goal(goal_handle_);
339+
if (should_cancel_goal()) {
340+
auto future_cancel = action_client_->async_cancel_goal(goal_handle_);
339341

340-
if (rclcpp::spin_until_future_complete(node_, future_cancel, server_timeout_) !=
341-
rclcpp::FutureReturnCode::SUCCESS)
342+
if (rclcpp::spin_until_future_complete(node_, future_cancel, server_timeout_) !=
343+
rclcpp::FutureReturnCode::SUCCESS)
344+
{
345+
RCLCPP_ERROR( node_->get_logger(),
346+
"Failed to cancel action server for %s", action_name_.c_str());
347+
}
348+
}
349+
}
350+
351+
template<class T> inline
352+
bool RosActionNode<T>::should_cancel_goal()
353+
{
354+
// Shut the node down if it is currently running
355+
if (status() != BT::NodeStatus::RUNNING) {
356+
return false;
357+
}
358+
359+
rclcpp::spin_some(node_);
360+
auto status = goal_handle_->get_status();
361+
362+
// Check if the goal is still executing
363+
if (status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED ||
364+
status == action_msgs::msg::GoalStatus::STATUS_EXECUTING)
342365
{
343-
RCLCPP_ERROR( node_->get_logger(),
344-
"Failed to cancel action server for %s", action_name_.c_str());
366+
return true;
345367
}
368+
369+
return false;
346370
}
347371

348372
} // namespace BT
349373

350-
#endif // BEHAVIOR_TREE_ROS2__BT_ACTION_NODE_HPP_
374+
#endif // BEHAVIOR_TREE_ROS2__BT_ACTION_NODE_HPP_

‎mep3_behavior/include/mep3_behavior/move_action.hpp

+8-3
Original file line numberDiff line numberDiff line change
@@ -135,6 +135,8 @@ namespace mep3_behavior
135135
// ala ce robot da leti :)
136136
if (!getInput("max_acceleration", max_acceleration_))
137137
max_velocity_ = 99999;
138+
if (!getInput("ignore_obstacles", ignore_obstacles_))
139+
ignore_obstacles_=true;
138140

139141
std::string table = this->config().blackboard->get<std::string>("table");
140142
double goal_offset;
@@ -148,15 +150,16 @@ namespace mep3_behavior
148150
{
149151
std::cout << "Translate to " << target_position_ \
150152
<< "m max_velocity="<<max_velocity_\
151-
<<" max_acceleration="<<max_acceleration_<<std::endl;
153+
<<" max_acceleration="<<max_acceleration_\
154+
<<" ignore_obstacles="<<ignore_obstacles_<<std::endl;
152155

153156

154157
goal.header.frame_id = "base_link";
155158
goal.target.x = target_position_;
156159
goal.type = mep3_msgs::action::Move::Goal::TYPE_TRANSLATE;
157160
goal.linear_properties.max_velocity = max_velocity_;
158161
goal.linear_properties.max_acceleration = max_acceleration_;
159-
goal.ignore_obstacles = false;
162+
goal.ignore_obstacles = ignore_obstacles_;
160163

161164
return true;
162165
}
@@ -167,7 +170,8 @@ namespace mep3_behavior
167170
BT::PortsList port_list = {
168171
BT::InputPort<double>("goal"),
169172
BT::InputPort<double>("max_velocity"),
170-
BT::InputPort<double>("max_acceleration")};
173+
BT::InputPort<double>("max_acceleration"),
174+
BT::InputPort<bool>("ignore_obstacles")};
171175

172176
// Dynamic parameters
173177
for (std::string table : BT::SharedBlackboard::access()->get<std::vector<std::string>>("predefined_tables"))
@@ -187,6 +191,7 @@ namespace mep3_behavior
187191
double target_position_;
188192
double max_velocity_;
189193
double max_acceleration_;
194+
bool ignore_obstacles_;
190195
};
191196

192197

‎mep3_behavior/src/mep3_behavior_tree.cpp

+11-2
Original file line numberDiff line numberDiff line change
@@ -116,8 +116,17 @@ int main(int argc, char **argv)
116116

117117
// Set color
118118
node->declare_parameter<std::string>("color", "blue");
119-
auto color = node->get_parameter("color");
120-
if (color.as_string() == "green")
119+
auto color = node->get_parameter("color").as_string();
120+
121+
//Adapt different start field to default color
122+
size_t found = color.find('_');
123+
if(found != std::string::npos){
124+
color.erase(found, color.size());
125+
}
126+
127+
std::cout<<"ERASED COLOR: "<<color<<found<<std::endl;
128+
129+
if (color == "green")
121130
blackboard->set("color", BT::TeamColor::GREEN);
122131
else
123132
blackboard->set("color", BT::TeamColor::BLUE);

‎mep3_behavior/strategies/big_blue.xml

+102-1
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,75 @@
22
<root BTCPP_format="4">
33
<BehaviorTree ID="big_blue">
44
<Sequence>
5-
<Wait duration="4"/>
5+
<WaitMatchStart state="2"/>
6+
<Wait name="START"
7+
duration="400"/>
8+
<JointPosition instance="m10"
9+
max_effort="0"
10+
max_velocity="220"
11+
position="183"
12+
tolerance="10"
13+
feedback_effort="feedback_effort"
14+
feedback_position="feedback_position"
15+
result="result"/>
16+
<Wait duration="5"/>
17+
<ForceSuccess>
18+
<Sequence _onSuccess="uradio := 1"
19+
_onFailure="uradio := 0">
20+
<ForceFailure>
21+
<JointPosition instance="m10"
22+
max_effort="0"
23+
max_velocity="220"
24+
position="183"
25+
tolerance="10"
26+
feedback_effort="feedback_effort"
27+
feedback_position="feedback_position"
28+
result="result"/>
29+
</ForceFailure>
30+
<JointPosition instance="m6"
31+
max_effort="0"
32+
max_velocity="220"
33+
position="150"
34+
tolerance="10"
35+
feedback_effort="feedback_effort"
36+
feedback_position="feedback_position"
37+
result="result"/>
38+
</Sequence>
39+
</ForceSuccess>
40+
<Sequence _skipIf="uradio == 1">
41+
<JointPosition instance="m8"
42+
max_effort="0"
43+
max_velocity="220"
44+
position="252"
45+
tolerance="10"
46+
feedback_effort="feedback_effort"
47+
feedback_position="feedback_position"
48+
result="result"/>
49+
<JointPosition instance="m6"
50+
max_effort="0"
51+
max_velocity="220"
52+
position="150"
53+
tolerance="10"
54+
feedback_effort="feedback_effort"
55+
feedback_position="feedback_position"
56+
result="result"/>
57+
</Sequence>
58+
<SubTree ID="move_stuck"
59+
color="{color}"
60+
goal="-0.64;-0.83;180"
61+
node="{node}"
62+
table="{table}"
63+
timeout="5000"
64+
type="0"/>
65+
<SubTree ID="gentle_attack"
66+
color="{color}"
67+
goal="-0.3;-0.83;180"
68+
node="{node}"
69+
table="{table}"/>
70+
<Translate goal="0.4"
71+
ignore_obstacles="false"
72+
max_velocity="0.8"
73+
max_acceleration="0.5"/>
674
<SubTree ID="move_stuck"
775
color="{color}"
876
goal="1.0;0.28;0"
@@ -15,10 +83,43 @@
1583

1684
<!-- Description of Node Models (used by Groot) -->
1785
<TreeNodesModel>
86+
<Action ID="JointPosition"
87+
editable="true">
88+
<input_port name="instance"/>
89+
<input_port name="max_effort"
90+
default="0"/>
91+
<input_port name="max_velocity"
92+
default="180"/>
93+
<input_port name="position"/>
94+
<input_port name="tolerance"
95+
default="10"/>
96+
<output_port name="feedback_effort"
97+
default="feedback_effort"/>
98+
<output_port name="feedback_position"
99+
default="feedback_position"/>
100+
<output_port name="result"
101+
default="result"/>
102+
</Action>
103+
<Action ID="Translate"
104+
editable="true">
105+
<input_port name="goal"
106+
default="0"/>
107+
<input_port name="ignore_obstacles"
108+
default="false"/>
109+
<input_port name="max_velocity"
110+
default="0.8"/>
111+
<inout_port name="max_acceleration"
112+
default="0.5"/>
113+
</Action>
18114
<Action ID="Wait"
19115
editable="true">
20116
<input_port name="duration"/>
21117
</Action>
118+
<Action ID="WaitMatchStart"
119+
editable="true">
120+
<input_port name="state"
121+
default="2">0 = unarmed; 1 = armed; 2 = started</input_port>
122+
</Action>
22123
</TreeNodesModel>
23124

24125
</root>

‎mep3_behavior/strategies/big_homologation.xml

+11-7
Original file line numberDiff line numberDiff line change
@@ -42,8 +42,9 @@
4242
color="{color}"
4343
table="{table}"/>
4444
<Translate goal="-0.5"
45-
max_acceleration="0.5"
46-
max_velocity="0.5"/>
45+
ignore_obstacles="false"
46+
max_velocity="0.5"
47+
max_acceleration="0.5"/>
4748
</Sequence>
4849
<SubTree ID="move_homologation"
4950
color="{color}"
@@ -54,9 +55,10 @@
5455
</TaskSequence>
5556
</Timeout>
5657
</ForceSuccess>
57-
<Translate goal="0.5"
58-
max_acceleration="0.5"
59-
max_velocity="0.5"/>
58+
<Translate goal="0.2"
59+
ignore_obstacles="false"
60+
max_velocity="0.5"
61+
max_acceleration="0.5"/>
6062
</Sequence>
6163
</ForceSuccess>
6264
</Timeout>
@@ -88,10 +90,12 @@
8890
editable="true">
8991
<input_port name="goal"
9092
default="0"/>
91-
<input_port name="max_acceleration"
92-
default="0.5"/>
93+
<input_port name="ignore_obstacles"
94+
default="false"/>
9395
<input_port name="max_velocity"
9496
default="0.8"/>
97+
<inout_port name="max_acceleration"
98+
default="0.5"/>
9599
</Action>
96100
<Action ID="Wait"
97101
editable="true">

0 commit comments

Comments
 (0)