Skip to content
Snippets Groups Projects
Commit e8373e73 authored by Timo Weberruß's avatar Timo Weberruß
Browse files

Add aron conversions for proxemics as part of human

parent fac615fb
No related branches found
No related tags found
1 merge request!109Social layers
......@@ -2,6 +2,40 @@
<AronTypeDefinition>
<GenerateTypes>
<Object name='armarx::navigation::human::arondto::ExponentialPenaltyModel'>
<ObjectChild key='minDistance'>
<Float />
</ObjectChild>
<ObjectChild key='epsilon'>
<Float />
</ObjectChild>
<ObjectChild key='exponent'>
<Float />
</ObjectChild>
</Object>
<Object name='armarx::navigation::human::arondto::ProxemicZone'>
<ObjectChild key='pose'>
<Pose />
</ObjectChild>
<ObjectChild key='ellipseA'>
<Float />
</ObjectChild>
<ObjectChild key='ellipseB'>
<Float />
</ObjectChild>
<ObjectChild key='penalty'>
<armarx::navigation::human::arondto::ExponentialPenaltyModel />
</ObjectChild>
<ObjectChild key='weight'>
<Float />
</ObjectChild>
<ObjectChild key='homotopicRelevance'>
<Bool />
</ObjectChild>
</Object>
<Object name='armarx::navigation::human::arondto::Human'>
<ObjectChild key='pose'>
<Pose />
......@@ -12,6 +46,11 @@
<ObjectChild key='detectionTime'>
<Time />
</ObjectChild>
<ObjectChild key='proxemicZones'>
<List>
<armarx::navigation::human::arondto::ProxemicZone />
</List>
</ObjectChild>
</Object>
<Object name='armarx::navigation::human::arondto::HumanGroup'>
......
......@@ -14,6 +14,15 @@ namespace armarx::navigation::human
dto.pose = conv::to3D(bo.pose).matrix();
dto.linearVelocity = conv::to3D(bo.linearVelocity);
dto.detectionTime = bo.detectionTime;
dto.proxemicZones = bo.proxemicZones |
ranges::views::transform(
[](const ProxemicZone& boZone) -> arondto::ProxemicZone
{
arondto::ProxemicZone dtoZone;
toAron(dtoZone, boZone);
return dtoZone;
}) |
ranges::to_vector;
}
void
......@@ -22,6 +31,15 @@ namespace armarx::navigation::human
bo.pose = conv::to2D(core::Pose(dto.pose));
bo.linearVelocity = conv::to2D(dto.linearVelocity);
bo.detectionTime = dto.detectionTime;
bo.proxemicZones = dto.proxemicZones |
ranges::views::transform(
[](const arondto::ProxemicZone& dtoZone) -> ProxemicZone
{
ProxemicZone boZone;
fromAron(dtoZone, boZone);
return boZone;
}) |
ranges::to_vector;
}
......@@ -66,4 +84,42 @@ namespace armarx::navigation::human
bo.detectionTime = dto.detectionTime;
}
void
toAron(arondto::ExponentialPenaltyModel& dto, const ExponentialPenaltyModel& bo)
{
dto.minDistance = bo.minDistance;
dto.epsilon = bo.epsilon;
dto.exponent = bo.exponent;
}
void
fromAron(const arondto::ExponentialPenaltyModel& dto, ExponentialPenaltyModel& bo)
{
bo.minDistance = dto.minDistance;
bo.epsilon = dto.epsilon;
bo.exponent = dto.exponent;
}
void
toAron(arondto::ProxemicZone& dto, const ProxemicZone& bo)
{
dto.pose = conv::to3D(bo.pose).matrix();
dto.ellipseA = bo.shape.a;
dto.ellipseB = bo.shape.b;
toAron(dto.penalty, bo.penalty);
dto.weight = bo.weight;
dto.homotopicRelevance = bo.homotopicRelevance;
}
void
fromAron(const arondto::ProxemicZone& dto, ProxemicZone& bo)
{
bo.pose = conv::to2D(core::Pose(dto.pose));
bo.shape = {.a = dto.ellipseA, .b = dto.ellipseB};
fromAron(dto.penalty, bo.penalty);
bo.weight = dto.weight;
bo.homotopicRelevance = dto.homotopicRelevance;
}
} // namespace armarx::navigation::human
......@@ -14,6 +14,7 @@
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
* @author Tobias Gröger ( tobias dot groeger at student dot kit dot edu )
* @author Timo Weberruß ( timo dot weberruss at student dot kit dot edu )
* @date 2022
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
......@@ -25,11 +26,15 @@ namespace armarx::navigation::human
{
struct Human;
struct HumanGroup;
struct ExponentialPenaltyModel;
struct ProxemicZone;
namespace arondto
{
struct Human;
struct HumanGroup;
struct ExponentialPenaltyModel;
struct ProxemicZone;
} // namespace arondto
......@@ -43,4 +48,10 @@ namespace armarx::navigation::human
void toAron(arondto::HumanGroup& dto, const HumanGroup& bo);
void fromAron(const arondto::HumanGroup& dto, HumanGroup& bo);
void toAron(arondto::ExponentialPenaltyModel& dto, const ExponentialPenaltyModel& bo);
void fromAron(const arondto::ExponentialPenaltyModel& dto, ExponentialPenaltyModel& bo);
void toAron(arondto::ProxemicZone& dto, const ProxemicZone& bo);
void fromAron(const arondto::ProxemicZone& dto, ProxemicZone& bo);
} // namespace armarx::navigation::human
......@@ -22,6 +22,7 @@ namespace armarx::navigation::human
other.detectionTime == this->detectionTime;
}
aron::data::DictPtr
Human::toAron() const
{
......@@ -42,6 +43,46 @@ namespace armarx::navigation::human
return bo;
}
aron::data::DictPtr
ExponentialPenaltyModel::toAron() const
{
arondto::ExponentialPenaltyModel dto;
human::toAron(dto, *this);
return dto.toAron();
}
ExponentialPenaltyModel
ExponentialPenaltyModel::FromAron(const aron::data::DictPtr& dict)
{
ARMARX_CHECK_NOT_NULL(dict);
arondto::ExponentialPenaltyModel dto;
dto.fromAron(dict);
ExponentialPenaltyModel bo;
fromAron(dto, bo);
return bo;
}
aron::data::DictPtr
ProxemicZone::toAron() const
{
arondto::ProxemicZone dto;
human::toAron(dto, *this);
return dto.toAron();
}
ProxemicZone
ProxemicZone::FromAron(const aron::data::DictPtr& dict)
{
ARMARX_CHECK_NOT_NULL(dict);
arondto::ProxemicZone dto;
dto.fromAron(dict);
ProxemicZone bo;
fromAron(dto, bo);
return bo;
}
aron::data::DictPtr
HumanGroup::toAron() const
......
......@@ -30,11 +30,42 @@
namespace armarx::navigation::human
{
struct LinearPenaltyModel
{
float minDistance; // [m]
float epsilon; // [m]
};
struct ExponentialPenaltyModel
{
float minDistance; // [m]
float epsilon; // [m]
float exponent;
aron::data::DictPtr toAron() const;
static ExponentialPenaltyModel FromAron(const aron::data::DictPtr& dict);
};
struct ProxemicZone
{
core::Pose2D pose;
shapes::Ellipse shape;
ExponentialPenaltyModel penalty;
float weight;
bool homotopicRelevance = true;
aron::data::DictPtr toAron() const;
static ProxemicZone FromAron(const aron::data::DictPtr& dict);
};
using ProxemicZones = std::vector<ProxemicZone>;
struct Human
{
core::Pose2D pose;
Eigen::Vector2f linearVelocity;
DateTime detectionTime;
ProxemicZones proxemicZones = {};
core::Pose2D estimateAt(const DateTime& time) const;
......@@ -59,28 +90,4 @@ namespace armarx::navigation::human
using HumanGroups = std::vector<HumanGroup>;
struct LinearPenaltyModel
{
float minDistance; // [m]
float epsilon; // [m]
};
struct ExponentialPenaltyModel
{
float minDistance; // [m]
float epsilon; // [m]
float exponent;
};
struct ProxemicZone
{
core::Pose2D pose;
shapes::Ellipse shape;
ExponentialPenaltyModel penalty;
float weight;
bool homotopicRelevance = true;
};
using ProxemicZones = std::vector<ProxemicZone>;
} // namespace armarx::navigation::human
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment