Skip to content

Commit

Permalink
Decouple the code for the CustomNodeParser
Browse files Browse the repository at this point in the history
  • Loading branch information
@Chi-EEE
Chi-EEE committed Apr 17, 2024
1 parent 0928d4d commit 37f044f
Show file tree
Hide file tree
Showing 11 changed files with 320 additions and 346 deletions.
268 changes: 9 additions & 259 deletions app/other/behaviour_tree_car/include/behaviour_tree/node/custom/CarCustomNodeParser.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,6 @@
#include "action/SetSpeed.hpp"
#include "action/SetWheelDirection.hpp"
#include "action/SetAngle.hpp"
#include "action/TurnAround.hpp"

#include "condition/SucceedOnAverageNearbyScan.hpp"
#include "condition/SucceedOnAnyNearbyScan.hpp"
Expand All @@ -45,272 +44,23 @@ namespace behaviour_tree::node::custom
switch (utils::hash(node_name))
{
case utils::hash("Action:PauseExecution"):
{
int ms = node.attribute("ms").as_int();
if (ms < 0)
{
return tl::unexpected(fmt::format(R"(Invalid ms: '{}' | Action:PauseExecution:['{}',{}])", ms, name_attribute, index));
}
return std::make_shared<custom::action::PauseExecution>(
custom::action::PauseExecution(
name_attribute,
ms));
}
return custom::action::PauseExecution::parse(node, index, name_attribute);
case utils::hash("Action:Print"):
{
return std::make_shared<custom::action::Print>(
custom::action::Print(
name_attribute,
node.attribute("text").as_string()));
}
return custom::action::Print::parse(node, index, name_attribute);
case utils::hash("Action:Drive"):
{
const int speed = node.attribute("speed").as_int();
if (speed < 0 || speed > 100)
{
return tl::unexpected(fmt::format(R"(Invalid speed: '{}' | Action:Drive:['{}',{}])", speed, name_attribute, index));
}
const tl::expected<custom::action::DirectionType, std::string> maybe_direction_type =
[&]()
{
const std::string direction_type_attribute = node.attribute("direction_type").as_string();
tl::expected<custom::action::DirectionType, std::string> result;
switch (utils::hash(direction_type_attribute))
{
case utils::hash("Forward"):
{
result = custom::action::DirectionType::Forward;
}
case utils::hash("Backward"):
{
result = custom::action::DirectionType::Backward;
}
default:
{
result = tl::make_unexpected(fmt::format(R"(Invalid direction_type: '{}' | Action:Drive:['{}',{}])", direction_type_attribute, name_attribute, index));
}
};
return result;
}();
if (!maybe_direction_type.has_value())
{
return tl::make_unexpected(maybe_direction_type.error());
}
return std::make_shared<custom::action::Drive>(
custom::action::Drive(
name_attribute,
speed,
maybe_direction_type.value()));
}
return custom::action::Drive::parse(node, index, name_attribute);
case utils::hash("Action:SetSpeed"):
{
const tl::expected<custom::action::WheelType, std::string> maybe_wheel_type = [&]()
{
const std::string wheel_type_attribute = node.attribute("wheel_type").as_string();
tl::expected<custom::action::WheelType, std::string> result;
switch (utils::hash(wheel_type_attribute))
{
case utils::hash("Left"):
{
result = custom::action::WheelType::Left;
break;
}
case utils::hash("Right"):
{
result = custom::action::WheelType::Right;
break;
}
case utils::hash("Both"):
{
result = custom::action::WheelType::Both;
break;
}
default:
{
result = tl::unexpected(fmt::format(R"(Invalid wheel_type: '{}' | Action:SetSpeed:['{}',{}])", wheel_type_attribute, name_attribute, index));
}
};
return result;
}();
if (!maybe_wheel_type.has_value())
{
return tl::make_unexpected(maybe_wheel_type.error());
}
const int speed = node.attribute("speed").as_int();
if (speed < 0 || speed > 100)
{
return tl::unexpected(fmt::format(R"(Invalid speed: '{}' | Action:SetSpeed:['{}',{}])", speed, name_attribute, index));
}
return std::make_shared<custom::action::SetSpeed>(custom::action::SetSpeed(name_attribute, maybe_wheel_type.value(), speed));
}
return custom::action::SetSpeed::parse(node, index, name_attribute);
case utils::hash("Action:SetAngle"):
{
const tl::expected<custom::action::ServoType, std::string> maybe_servo_type = [&]()
{
const std::string servo_type_attribute = node.attribute("servo_type").as_string();
tl::expected<custom::action::ServoType, std::string> result;
switch (utils::hash(servo_type_attribute))
{
case utils::hash("FrontWheels"):
{
result = custom::action::ServoType::FrontWheels;
break;
}
case utils::hash("CameraServo1"):
{
result = custom::action::ServoType::CameraServo1;
break;
}
case utils::hash("CameraServo2"):
{
result = custom::action::ServoType::CameraServo2;
break;
}
default:
{
result = tl::unexpected(fmt::format(R"(Invalid servo_type: '{}' | Action:SetAngle:['{}',{}])", servo_type_attribute, name_attribute, index));
}
};
return result;
}();
const int angle = node.attribute("angle").as_int();
if (angle < 0 || angle > 180)
return tl::unexpected(fmt::format(R"(Invalid angle: '{}' | Action:SetAngle:['{}',{}])", angle, name_attribute, index));
return std::make_shared<custom::action::SetAngle>(custom::action::SetAngle(name_attribute, maybe_servo_type.value(), angle));
}
return custom::action::SetAngle::parse(node, index, name_attribute);
case utils::hash("Action:SetWheelDirection"):
{
const tl::expected<custom::action::WheelType, std::string> maybe_wheel_type = [&]()
{
const std::string wheel_type_attribute = node.attribute("wheel_type").as_string();
tl::expected<custom::action::WheelType, std::string> result;
switch (utils::hash(wheel_type_attribute))
{
case utils::hash("Left"):
{
result = custom::action::WheelType::Left;
break;
}
case utils::hash("Right"):
{
result = custom::action::WheelType::Right;
break;
}
case utils::hash("Both"):
{
result = custom::action::WheelType::Both;
break;
}
default:
{
result = tl::unexpected(fmt::format(R"(Invalid wheel_type: '{}' | Action:SetWheelDirection:['{}',{}])", wheel_type_attribute, name_attribute, index));
}
};
return result;
}();
if (!maybe_wheel_type.has_value())
{
return tl::make_unexpected(maybe_wheel_type.error());
}
const tl::expected<custom::action::DirectionType, std::string> maybe_direction_type =
[&]()
{
const std::string direction_type_attribute = node.attribute("direction_type").as_string();
tl::expected<custom::action::DirectionType, std::string> result;
switch (utils::hash(direction_type_attribute))
{
case utils::hash("Forward"):
{
result = custom::action::DirectionType::Forward;
}
case utils::hash("Backward"):
{
result = custom::action::DirectionType::Backward;
}
default:
{
result = tl::make_unexpected(fmt::format(R"(Invalid direction_type: '{}' | Action:Drive:['{}',{}])", direction_type_attribute, name_attribute, index));
}
};
return result;
}();
if (!maybe_direction_type.has_value())
{
return tl::make_unexpected(maybe_direction_type.error());
}
return std::make_shared<custom::action::SetWheelDirection>(custom::action::SetWheelDirection(name_attribute, maybe_wheel_type.value(), maybe_direction_type.value()));
}
return custom::action::SetWheelDirection::parse(node, index, name_attribute);
case utils::hash("Condition:SucceedOnAverageNearbyScan"):
{
const int min_angle = node.attribute("min_angle").as_int();
if (min_angle < 0 || min_angle > 360)
return tl::unexpected(fmt::format(R"(Invalid min_angle: '{}' | Condition:SucceedOnAverageNearbyScan:['{}',{}])", min_angle, name_attribute, index));
const int max_angle = node.attribute("max_angle").as_int();
if (max_angle < 0 || max_angle > 360)
return tl::unexpected(fmt::format(R"(Invalid max_angle: '{}' | Condition:SucceedOnAverageNearbyScan:['{}',{}])", max_angle, name_attribute, index));
const double cm = node.attribute("cm").as_double();
if (cm < 0)
return tl::unexpected(fmt::format(R"(Invalid cm: '{}' | Condition:SucceedOnAverageNearbyScan:['{}',{}])", cm, name_attribute, index));
const int minimum_measure_amount_used = node.attribute("minimum_measure_amount_used").as_int();
if (minimum_measure_amount_used < 0)
return tl::unexpected(fmt::format(R"(Invalid minimum_measure_amount_used: '{}' | Condition:SucceedOnAverageNearbyScan:['{}',{}])", minimum_measure_amount_used, name_attribute, index));
return std::make_shared<custom::condition::SucceedOnAverageNearbyScan>(
custom::condition::SucceedOnAverageNearbyScan(
name_attribute,
min_angle,
max_angle,
cm,
minimum_measure_amount_used));
}
return custom::condition::SucceedOnAverageNearbyScan::parse(node, index, name_attribute);
case utils::hash("Condition:SucceedOnAnyNearbyScan"):
{
const int min_angle = node.attribute("min_angle").as_int();
if (min_angle < 0 || min_angle > 360)
return tl::unexpected(fmt::format(R"(Invalid min_angle: '{}' | Condition:SucceedOnAnyNearbyScan:['{}',{}])", min_angle, name_attribute, index));
const int max_angle = node.attribute("max_angle").as_int();
if (max_angle < 0 || max_angle > 360)
return tl::unexpected(fmt::format(R"(Invalid max_angle: '{}' | Condition:SucceedOnAnyNearbyScan:['{}',{}])", max_angle, name_attribute, index));
const double cm = node.attribute("cm").as_double();
if (cm < 0)
return tl::unexpected(fmt::format(R"(Invalid cm: '{}' | Condition:SucceedOnAnyNearbyScan:['{}',{}])", cm, name_attribute, index));
return std::make_shared<custom::condition::SucceedOnAnyNearbyScan>(
custom::condition::SucceedOnAnyNearbyScan(
name_attribute,
min_angle,
max_angle,
cm));
}
return custom::condition::SucceedOnAnyNearbyScan::parse(node, index, name_attribute);
case utils::hash("Condition:SucceedOnAverageColour"):
{
const std::string hex = node.attribute("hex").as_string();
const double percentage = node.attribute("percentage").as_double();
if (percentage < 0 || percentage > 100)
return tl::unexpected(fmt::format(R"(Invalid percentage: '{}' | Condition:SucceedOnAverageColour:['{}',{}])", percentage, name_attribute, index));
return std::make_shared<custom::condition::SucceedOnAverageColour>(
custom::condition::SucceedOnAverageColour(
name_attribute,
hex,
percentage));
}
// case utils::hash("Action:TurnAround"):
// {
// const std::string direction_type_attribute = node.attribute("direction_type").as_string();
// switch (utils::hash(direction_type_attribute))
// {
// case utils::hash("Clockwise"):
// {
// return std::make_shared<custom::action::TurnAround>(custom::action::TurnAround(name_attribute, custom::action::ClockDirectionType::Clockwise));
// }
// case utils::hash("AntiClockwise"):
// {
// return std::make_shared<custom::action::TurnAround>(custom::action::TurnAround(name_attribute, custom::action::ClockDirectionType::AntiClockwise));
// }
// default:
// {
// return tl::unexpected(fmt::format(R"(Invalid direction_type: '{}' | Action:TurnAround:['{}',{}])", direction_type_attribute, name_attribute, index));
// }
// }
// }
return custom::condition::SucceedOnAverageColour::parse(node, index, name_attribute);
default:
{
return tl::unexpected(fmt::format(R"(Invalid custom node type: '{}' | {}:['{}',{}])", node_name, node_name, name_attribute, index));
Expand Down
42 changes: 42 additions & 0 deletions app/other/behaviour_tree_car/include/behaviour_tree/node/custom/action/Drive.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,8 @@

#pragma once

#include <fmt/format.h>

#include "behaviour_tree/node/custom/CustomNode.hpp"

#include "behaviour_tree/Context.h"
Expand All @@ -21,6 +23,46 @@ namespace behaviour_tree::node::custom::action
{
}

const static tl::expected<std::shared_ptr<Drive>, std::string> parse(const pugi::xml_node &node, const int index, const std::string &name_attribute)
{
const int speed = node.attribute("speed").as_int();
if (speed < 0 || speed > 100)
{
return tl::unexpected(fmt::format(R"(Invalid speed: '{}' | Action:Drive:['{}',{}])", speed, name_attribute, index));
}
const tl::expected<DirectionType, std::string> maybe_direction_type =
[&]()
{
const std::string direction_type_attribute = node.attribute("direction_type").as_string();
tl::expected<DirectionType, std::string> result;
switch (utils::hash(direction_type_attribute))
{
case utils::hash("Forward"):
{
result = DirectionType::Forward;
}
case utils::hash("Backward"):
{
result = DirectionType::Backward;
}
default:
{
result = tl::make_unexpected(fmt::format(R"(Invalid direction_type: '{}' | Action:Drive:['{}',{}])", direction_type_attribute, name_attribute, index));
}
};
return result;
}();
if (!maybe_direction_type.has_value())
{
return tl::make_unexpected(maybe_direction_type.error());
}
return std::make_shared<Drive>(
Drive(
name_attribute,
speed,
maybe_direction_type.value()));
}

const Status run(const int tick_count, std::shared_ptr<Context> context) final override
{
#ifndef BEHAVIOUR_TREE_DISABLE_RUN
Expand Down
20 changes: 18 additions & 2 deletions app/other/behaviour_tree_car/include/behaviour_tree/node/custom/action/PauseExecution.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
#pragma once

#include <chrono>
#include <fmt/format.h>

#include "behaviour_tree/node/custom/CustomNode.hpp"

Expand All @@ -18,13 +19,28 @@ namespace behaviour_tree::node::custom::action
{
}

void start(std::shared_ptr<Context> context) final override {
const static tl::expected<std::shared_ptr<PauseExecution>, std::string> parse(const pugi::xml_node &node, const int index, const std::string &name_attribute)
{
int ms = node.attribute("ms").as_int();
if (ms < 0)
{
return tl::unexpected(fmt::format(R"(Invalid ms: '{}' | Action:PauseExecution:['{}',{}])", ms, name_attribute, index));
}
return std::make_shared<custom::action::PauseExecution>(
custom::action::PauseExecution(
name_attribute,
ms));
}

void start(std::shared_ptr<Context> context) final override
{
#ifndef BEHAVIOUR_TREE_DISABLE_RUN
this->start_time = std::chrono::steady_clock::now();
#endif
}

void finish(std::shared_ptr<Context> context) final override {
void finish(std::shared_ptr<Context> context) final override
{
}

const Status run(const int tick_count, std::shared_ptr<Context> context) final override
Expand Down
8 changes: 8 additions & 0 deletions app/other/behaviour_tree_car/include/behaviour_tree/node/custom/action/Print.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,14 @@ namespace behaviour_tree::node::custom::action
{
}

const static tl::expected<std::shared_ptr<Print>, std::string> parse(const pugi::xml_node &node, const int index, const std::string &name_attribute)
{
return std::make_shared<Print>(
Print(
name_attribute,
node.attribute("text").as_string()));
}

const Status run(const int tick_count, std::shared_ptr<Context> context) final override
{
#ifndef BEHAVIOUR_TREE_DISABLE_RUN
Expand Down
Loading

0 comments on commit 37f044f

Please sign in to comment.