Skip to content

Commit

Permalink
Add smallest measure amount used
Browse files Browse the repository at this point in the history
  • Loading branch information
@Chi-EEE
Chi-EEE committed Apr 16, 2024
1 parent 01169c1 commit 3b67872
Show file tree
Hide file tree
Showing 4 changed files with 120 additions and 92 deletions.
3 changes: 2 additions & 1 deletion app/admin_panel/src/lib/CodeBox_Constants.js
View file
Original file line number Diff line number Diff line change
Expand Up @@ -818,7 +818,8 @@ export const xml_schema = {
values: range_0_360,
completion: { type: "keyword" },
},
"cm", // Every unit is 0.15 cms
"cm",
"smallest_measure_amount_used",
],
completion: { type: "keyword" },
},
Expand Down
1 change: 1 addition & 0 deletions app/frontend/src/lib/CodeBox_Constants.js
View file
Original file line number Diff line number Diff line change
Expand Up @@ -457,6 +457,7 @@ export const xml_schema = {
completion: { type: "keyword" },
},
"cm",
"smallest_measure_amount_used",
],
completion: { type: "keyword" },
},
Expand Down
46 changes: 25 additions & 21 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 @@ -176,32 +176,36 @@ namespace behaviour_tree::node::custom
const double cm = node.attribute("cm").as_double();
if (cm < 0)
return tl::unexpected(fmt::format(R"(Invalid cm: '{}' | Condition:SuccessOnAverageNearbyScan:['{}',{}])", cm, name_attribute, index));
return std::make_shared<custom::condition::SuccessOnAverageNearbyScan>(
const int measures_used = node.attribute("measures_used").as_int();
if (measures_used < 0)
return tl::unexpected(fmt::format(R"(Invalid measures_used: '{}' | Condition:SuccessOnAverageNearbyScan:['{}',{}])", measures_used, name_attribute, index));
return std::make_shared<custom::condition::SuccessOnAverageNearbyScan>(
custom::condition::SuccessOnAverageNearbyScan(
name_attribute,
min_angle,
max_angle,
cm));
}
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));
}
}
cm,
measures_used));
}
// 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));
// }
// }
// }
default:
{
return tl::unexpected(fmt::format(R"(Invalid custom node type: '{}' | {}:['{}',{}])", node_name, node_name, name_attribute, index));
Expand Down
162 changes: 92 additions & 70 deletions ...iour_tree_car/include/behaviour_tree/node/custom/condition/SuccessOnAverageNearbyScan.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,78 +13,100 @@

namespace behaviour_tree::node::custom::condition
{
constexpr double CM_TO_DISTANCE = 15.151515151515151515151515151515151515151515151515151515;
class SuccessOnAverageNearbyScan final : public CustomNode
{
public:
SuccessOnAverageNearbyScan(const std::string& name, const int min_angle, const int max_angle, const double& cm) :
CustomNode(name),
min_angle(min_angle),
max_angle(max_angle),
cm(cm),
average_distance_unit(cm * CM_TO_DISTANCE)
{
}

const Status run(const int tick_count, std::shared_ptr<Context> context) final override
{
constexpr double CM_TO_DISTANCE = 15.151515151515151515151515151515151515151515151515151515;
class SuccessOnAverageNearbyScan final : public CustomNode
{
public:
SuccessOnAverageNearbyScan(const std::string &name, const int min_angle, const int max_angle, const double cm, const int smallest_measure_amount_used) : CustomNode(name),
min_angle(min_angle),
max_angle(max_angle),
cm(cm),
smallest_measure_amount_used(smallest_measure_amount_used),
average_distance_unit(cm * CM_TO_DISTANCE)
{
}

const Status run(const int tick_count, std::shared_ptr<Context> context) final override
{
#ifndef BEHAVIOUR_TREE_DISABLE_RUN
std::shared_ptr<CarContext> car_context = std::dynamic_pointer_cast<CarContext>(context);
auto car_system = car_context->getCarSystem();
double total_distance = 0.0;
int angles_between_count = 0;
for (auto &measure : car_system->getDeviceManager()->getLidarDevice()->getScanData())
{
if (measure.angle > this->getMinAngle() && measure.angle < this->getMaxAngle())
{
total_distance += measure.distance;
++angles_between_count;
}
}
if (angles_between_count > 0)
{
double average_distance_unit = total_distance / angles_between_count;
spdlog::info("Average Distance: {}; Average Distance Unit: {}", average_distance_unit, this->getAverageDistanceUnit());
if (average_distance_unit < this->getAverageDistanceUnit())
{
return Status::Success;
}
}
std::shared_ptr<CarContext> car_context = std::dynamic_pointer_cast<CarContext>(context);
auto car_system = car_context->getCarSystem();
double total_distance = 0.0;
int angles_between_count = 0;
std::vector<Measure> scan_data = car_system->getDeviceManager()->getLidarDevice()->getScanData();
if (this->smallest_measure_amount_used > 0)
{
std::vector<Measure> smallest_measures;
std::partial_sort(scan_data.begin(), scan_data.begin() + this->smallest_measure_amount_used, scan_data.end(),
[](const Measure &a, const Measure &b)
{ return a.distance < b.distance; });
smallest_measures.insert(smallest_measures.end(), scan_data.begin(), scan_data.begin() + this->smallest_measure_amount_used);
scan_data = smallest_measures;
}
for (auto &measure : scan_data)
{
if (measure.angle > this->getMinAngle() && measure.angle < this->getMaxAngle())
{
total_distance += measure.distance;
++angles_between_count;
}
}
if (angles_between_count > 0)
{
double average_distance_unit = total_distance / angles_between_count;
if (average_distance_unit < this->getAverageDistanceUnit())
{
return Status::Success;
}
}
#endif // !BEHAVIOUR_TREE_DISABLE_RUN
return Status::Failure;
}

const int getMinAngle() const {
return this->min_angle;
}

const int getMaxAngle() const {
return this->max_angle;
}

const double& getAverageDistanceUnit() const {
return this->average_distance_unit;
}

const double& getCM() const {
return this->cm;
}

const std::string toString() const final override {
const std::string& name = this->getName();
if (name != "")
return fmt::format(R"(<Condition:SuccessOnAverageNearbyScan name='{}' min_angle='{}' max_angle='{}' cm='{}'/>)", name, this->getMinAngle(), this->getMaxAngle(), this->getCM());
else
return fmt::format(R"(<Condition:SuccessOnAverageNearbyScan min_angle='{}' max_angle='{}' cm='{}'/>)", this->getMinAngle(), this->getMaxAngle(), this->getCM());
}

private:
const int min_angle;
const int max_angle;

const double cm;
const double average_distance_unit;
};
return Status::Failure;
}

const int getMinAngle() const
{
return this->min_angle;
}

const int getMaxAngle() const
{
return this->max_angle;
}

const double getAverageDistanceUnit() const
{
return this->average_distance_unit;
}

const double getCentimeters() const
{
return this->cm;
}

const int getSmallestMeasureAmountUsed() const
{
return this->smallest_measure_amount_used;
}

const std::string toString() const final override
{
const std::string &name = this->getName();
if (name != "")
return fmt::format(R"(<Condition:SuccessOnAverageNearbyScan name='{}' min_angle='{}' max_angle='{}' cm='{}' smallest_measure_amount_used='{}'/>)", name, this->getMinAngle(), this->getMaxAngle(), this->getCentimeters(), this->getSmallestMeasureAmountUsed());
else
return fmt::format(R"(<Condition:SuccessOnAverageNearbyScan min_angle='{}' max_angle='{}' cm='{}' smallest_measure_amount_used='{}'/>)", this->getMinAngle(), this->getMaxAngle(), this->getCentimeters(), this->getSmallestMeasureAmountUsed());
}

private:
const int min_angle;
const int max_angle;

const double cm;

const int smallest_measure_amount_used;

const double average_distance_unit;
};
}

#endif

0 comments on commit 3b67872

Please sign in to comment.