Skip to content

Commit

Permalink
Bug fix and new features (#280)
Browse files Browse the repository at this point in the history 
* 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
  • Loading branch information
@MarijaGolubovic
MarijaGolubovic authored May 26, 2023
1 parent 8d314ce commit 3cebe0b
Show file tree
Hide file tree
Showing 50 changed files with 5,135 additions and 741 deletions.
36 changes: 30 additions & 6 deletions mep3_behavior/include/mep3_behavior/bt_action_node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -102,6 +102,7 @@ class RosActionNode : public BT::ActionNodeBase
void cancelGoal();

protected:
bool should_cancel_goal();

std::shared_ptr<rclcpp::Node> node_;
std::string action_name_;
Expand Down Expand Up @@ -335,16 +336,39 @@ template<class T> inline
if (!goal_handle_)
return;

auto future_cancel = action_client_->async_cancel_goal(goal_handle_);
if (should_cancel_goal()) {
auto future_cancel = action_client_->async_cancel_goal(goal_handle_);

if (rclcpp::spin_until_future_complete(node_, future_cancel, server_timeout_) !=
rclcpp::FutureReturnCode::SUCCESS)
if (rclcpp::spin_until_future_complete(node_, future_cancel, server_timeout_) !=
rclcpp::FutureReturnCode::SUCCESS)
{
RCLCPP_ERROR( node_->get_logger(),
"Failed to cancel action server for %s", action_name_.c_str());
}
}
}

template<class T> inline
bool RosActionNode<T>::should_cancel_goal()
{
// Shut the node down if it is currently running
if (status() != BT::NodeStatus::RUNNING) {
return false;
}

rclcpp::spin_some(node_);
auto status = goal_handle_->get_status();

// Check if the goal is still executing
if (status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED ||
status == action_msgs::msg::GoalStatus::STATUS_EXECUTING)
{
RCLCPP_ERROR( node_->get_logger(),
"Failed to cancel action server for %s", action_name_.c_str());
return true;
}

return false;
}

} // namespace BT

#endif // BEHAVIOR_TREE_ROS2__BT_ACTION_NODE_HPP_
#endif // BEHAVIOR_TREE_ROS2__BT_ACTION_NODE_HPP_
11 changes: 8 additions & 3 deletions mep3_behavior/include/mep3_behavior/move_action.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -135,6 +135,8 @@ namespace mep3_behavior
// ala ce robot da leti :)
if (!getInput("max_acceleration", max_acceleration_))
max_velocity_ = 99999;
if (!getInput("ignore_obstacles", ignore_obstacles_))
ignore_obstacles_=true;

std::string table = this->config().blackboard->get<std::string>("table");
double goal_offset;
Expand All @@ -148,15 +150,16 @@ namespace mep3_behavior
{
std::cout << "Translate to " << target_position_ \
<< "m max_velocity="<<max_velocity_\
<<" max_acceleration="<<max_acceleration_<<std::endl;
<<" max_acceleration="<<max_acceleration_\
<<" ignore_obstacles="<<ignore_obstacles_<<std::endl;


goal.header.frame_id = "base_link";
goal.target.x = target_position_;
goal.type = mep3_msgs::action::Move::Goal::TYPE_TRANSLATE;
goal.linear_properties.max_velocity = max_velocity_;
goal.linear_properties.max_acceleration = max_acceleration_;
goal.ignore_obstacles = false;
goal.ignore_obstacles = ignore_obstacles_;

return true;
}
Expand All @@ -167,7 +170,8 @@ namespace mep3_behavior
BT::PortsList port_list = {
BT::InputPort<double>("goal"),
BT::InputPort<double>("max_velocity"),
BT::InputPort<double>("max_acceleration")};
BT::InputPort<double>("max_acceleration"),
BT::InputPort<bool>("ignore_obstacles")};

// Dynamic parameters
for (std::string table : BT::SharedBlackboard::access()->get<std::vector<std::string>>("predefined_tables"))
Expand All @@ -187,6 +191,7 @@ namespace mep3_behavior
double target_position_;
double max_velocity_;
double max_acceleration_;
bool ignore_obstacles_;
};


Expand Down
13 changes: 11 additions & 2 deletions mep3_behavior/src/mep3_behavior_tree.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -116,8 +116,17 @@ int main(int argc, char **argv)

// Set color
node->declare_parameter<std::string>("color", "blue");
auto color = node->get_parameter("color");
if (color.as_string() == "green")
auto color = node->get_parameter("color").as_string();

//Adapt different start field to default color
size_t found = color.find('_');
if(found != std::string::npos){
color.erase(found, color.size());
}

std::cout<<"ERASED COLOR: "<<color<<found<<std::endl;

if (color == "green")
blackboard->set("color", BT::TeamColor::GREEN);
else
blackboard->set("color", BT::TeamColor::BLUE);
Expand Down
103 changes: 102 additions & 1 deletion mep3_behavior/strategies/big_blue.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,75 @@
<root BTCPP_format="4">
<BehaviorTree ID="big_blue">
<Sequence>
<Wait duration="4"/>
<WaitMatchStart state="2"/>
<Wait name="START"
duration="400"/>
<JointPosition instance="m10"
max_effort="0"
max_velocity="220"
position="183"
tolerance="10"
feedback_effort="feedback_effort"
feedback_position="feedback_position"
result="result"/>
<Wait duration="5"/>
<ForceSuccess>
<Sequence _onSuccess="uradio := 1"
_onFailure="uradio := 0">
<ForceFailure>
<JointPosition instance="m10"
max_effort="0"
max_velocity="220"
position="183"
tolerance="10"
feedback_effort="feedback_effort"
feedback_position="feedback_position"
result="result"/>
</ForceFailure>
<JointPosition instance="m6"
max_effort="0"
max_velocity="220"
position="150"
tolerance="10"
feedback_effort="feedback_effort"
feedback_position="feedback_position"
result="result"/>
</Sequence>
</ForceSuccess>
<Sequence _skipIf="uradio == 1">
<JointPosition instance="m8"
max_effort="0"
max_velocity="220"
position="252"
tolerance="10"
feedback_effort="feedback_effort"
feedback_position="feedback_position"
result="result"/>
<JointPosition instance="m6"
max_effort="0"
max_velocity="220"
position="150"
tolerance="10"
feedback_effort="feedback_effort"
feedback_position="feedback_position"
result="result"/>
</Sequence>
<SubTree ID="move_stuck"
color="{color}"
goal="-0.64;-0.83;180"
node="{node}"
table="{table}"
timeout="5000"
type="0"/>
<SubTree ID="gentle_attack"
color="{color}"
goal="-0.3;-0.83;180"
node="{node}"
table="{table}"/>
<Translate goal="0.4"
ignore_obstacles="false"
max_velocity="0.8"
max_acceleration="0.5"/>
<SubTree ID="move_stuck"
color="{color}"
goal="1.0;0.28;0"
Expand All @@ -15,10 +83,43 @@

<!-- Description of Node Models (used by Groot) -->
<TreeNodesModel>
<Action ID="JointPosition"
editable="true">
<input_port name="instance"/>
<input_port name="max_effort"
default="0"/>
<input_port name="max_velocity"
default="180"/>
<input_port name="position"/>
<input_port name="tolerance"
default="10"/>
<output_port name="feedback_effort"
default="feedback_effort"/>
<output_port name="feedback_position"
default="feedback_position"/>
<output_port name="result"
default="result"/>
</Action>
<Action ID="Translate"
editable="true">
<input_port name="goal"
default="0"/>
<input_port name="ignore_obstacles"
default="false"/>
<input_port name="max_velocity"
default="0.8"/>
<inout_port name="max_acceleration"
default="0.5"/>
</Action>
<Action ID="Wait"
editable="true">
<input_port name="duration"/>
</Action>
<Action ID="WaitMatchStart"
editable="true">
<input_port name="state"
default="2">0 = unarmed; 1 = armed; 2 = started</input_port>
</Action>
</TreeNodesModel>

</root>
18 changes: 11 additions & 7 deletions mep3_behavior/strategies/big_homologation.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,8 +42,9 @@
color="{color}"
table="{table}"/>
<Translate goal="-0.5"
max_acceleration="0.5"
max_velocity="0.5"/>
ignore_obstacles="false"
max_velocity="0.5"
max_acceleration="0.5"/>
</Sequence>
<SubTree ID="move_homologation"
color="{color}"
Expand All @@ -54,9 +55,10 @@
</TaskSequence>
</Timeout>
</ForceSuccess>
<Translate goal="0.5"
max_acceleration="0.5"
max_velocity="0.5"/>
<Translate goal="0.2"
ignore_obstacles="false"
max_velocity="0.5"
max_acceleration="0.5"/>
</Sequence>
</ForceSuccess>
</Timeout>
Expand Down Expand Up @@ -88,10 +90,12 @@
editable="true">
<input_port name="goal"
default="0"/>
<input_port name="max_acceleration"
default="0.5"/>
<input_port name="ignore_obstacles"
default="false"/>
<input_port name="max_velocity"
default="0.8"/>
<inout_port name="max_acceleration"
default="0.5"/>
</Action>
<Action ID="Wait"
editable="true">
Expand Down
Loading

0 comments on commit 3cebe0b

Please sign in to comment.