typename Generic_obj_space<SPACE>::Capability
Generic_obj_space<SPACE>::lookup(Address virt)
{
Capability *c = get_cap(virt);
if (EXPECT_FALSE(!c))
return Capability(0); // void
return *c;
}
typename Obj_space_phys<SPACE>::Capability __attribute__((__flatten__))
Obj_space_phys<SPACE>::lookup(Cap_index virt)
{
Capability *c = get_cap(virt);
if (EXPECT_FALSE(!c))
return Capability(0); // void
return *c;
}
TEST(ReachabilitySphere, convertToCapability)
{
Capability capability(CONE, 70.0, 55.0, 25.0);
ReachabilitySphere sphere = createReachabilitySphereFromCapability(capability, 500);
Capability testCapability = sphere.convertToCapability();
ASSERT_TRUE(testCapability.getType() == CONE);
ASSERT_TRUE(std::abs(testCapability.getPhi() - capability.getPhi()) < 2.0);
ASSERT_TRUE(std::abs(testCapability.getTheta() - capability.getTheta()) < 2.0);
ASSERT_TRUE(std::abs(testCapability.getHalfOpeningAngle() - capability.getHalfOpeningAngle()) < 2.0);
ASSERT_EQ(0.0, testCapability.getShapeFitError());
capability = Capability(CYLINDER_1, 70.0, 55.0, 25.0);
sphere = createReachabilitySphereFromCapability(capability, 10000);
testCapability = sphere.convertToCapability();
ASSERT_TRUE(testCapability.getType() == CYLINDER_1);
ASSERT_TRUE(std::abs(testCapability.getPhi() - capability.getPhi()) < 2.0);
ASSERT_TRUE(std::abs(testCapability.getTheta() - capability.getTheta()) < 2.0);
ASSERT_TRUE(std::abs(testCapability.getHalfOpeningAngle() - capability.getHalfOpeningAngle()) < 2.0);
// ASSERT_EQ(0.0, testCapability.getShapeFitError());
// TODO: improve PCA to get correct axis (for now SFE could be 0.0 but is higher)
capability = Capability(CYLINDER_2, 70.0, 55.0, 25.0);
sphere = createReachabilitySphereFromCapability(capability, 10000);
testCapability = sphere.convertToCapability();
ASSERT_TRUE(testCapability.getType() == CYLINDER_2);
ASSERT_TRUE(std::abs(testCapability.getPhi() - capability.getPhi()) < 2.0);
ASSERT_TRUE(std::abs(testCapability.getTheta() - capability.getTheta()) < 2.0);
ASSERT_TRUE(std::abs(testCapability.getHalfOpeningAngle() - capability.getHalfOpeningAngle()) < 2.0);
// ASSERT_EQ(0.0, testCapability.getShapeFitError());
}
typename Obj_space_virt<SPACE>::Capability __attribute__((__flatten__))
Obj_space_virt<SPACE>::lookup(Cap_index virt)
{
Capability *c;
virt &= Cap_index(~(~0UL << Whole_space));
if (SPACE::mem_space(this) == Mem_space::current_mem_space(current_cpu()))
c = reinterpret_cast<Capability*>(cap_virt(virt));
else
c = get_cap(virt);
if (EXPECT_FALSE(!c))
return Capability(0); // void
return Mem_layout::read_special_safe(c);
}
typename Generic_obj_space<SPACE>::Capability
Generic_obj_space<SPACE>::lookup(Address virt)
{
Capability *c;
virt &= ~(~0UL << Whole_space);
if (mem_space() == Mem_space::current_mem_space(current_cpu()))
c = reinterpret_cast<Capability*>(cap_virt(virt));
else
c = get_cap(virt);
if (EXPECT_FALSE(!c))
return Capability(0); // void
return Mem_layout::read_special_safe(c);
}
XtrazData::XtrazData(const QString &body, const Cookie &cookie) :
Tlv2711(MsgPlugin, 0, 0, 1, cookie)
{
appendEmptyPacket();
// Plugin type ID
append<quint16>(0x04f, LittleEndian); // Length
append(Capability(0x3b60b3ef, 0xd82a6c45, 0xa4e09c5a, 0x5e67e865)); // type: xtraz script
append<quint16>(xtrazNotify, LittleEndian); // Function ID
append<quint32>(0x002a, LittleEndian); // Request type
append(QString("Script Plug-in: Remote Notification Arrive"));
// unknown
append<quint32>(0x00000100);
append<quint32>(0x00000000);
append<quint32>(0x00000000);
append<quint16>(0x0000);
append<quint8>(0x00);
// data
DataUnit data;
data.append<quint32>(body, LittleEndian);
append<quint32>(data.data(), LittleEndian);
}
bool ReachabilityDummyInterface::isReachable(const octomath::Pose6D &pose) const
{
Capability cap;
if (pose.y() > 0.04 && pose.y() < 0.06 && pose.z() > 0.44 && pose.z() < 0.46)
{
if (pose.x() > 0.14 && pose.x() < 0.16)
{
cap = Capability(SPHERE, 0.0, 0.0, 0.0);
}
else if (pose.x() > 0.34 && pose.x() < 0.36)
{
cap = Capability(CONE, 0.0, 90.0, 15.0);
}
else if (pose.x() > 0.54 && pose.x() < 0.56)
{
cap = Capability(CONE, 90.0, 0.0, 45.0);
}
else if (pose.x() > 0.74 && pose.x() < 0.76)
{
cap = Capability(CONE, 90.0, 0.0, 125.0);
}
else if (pose.x() > 0.94 && pose.x() < 0.96)
{
cap = Capability(CONE, 180.0, 120.0, 70.0);
}
else if (pose.x() > 1.14 && pose.x() < 1.16)
{
cap = Capability(CYLINDER_1, 0.0, 0.0, 10.0);
}
else if (pose.x() > 1.34 && pose.x() < 1.36)
{
cap = Capability(CYLINDER_1, 0.0, 0.0, 90.0);
}
else if (pose.x() > 1.54 && pose.x() < 1.56)
{
cap = Capability(CYLINDER_1, 40.0, 50.0, 45.0);
}
else if (pose.x() > 1.74 && pose.x() < 1.76)
{
cap = Capability(CYLINDER_2, 0.0, 0.0, 10.0);
}
else if (pose.x() > 1.94 && pose.x() < 1.96)
{
cap = Capability(CYLINDER_2, 0.0, 0.0, 45.0);
}
else if (pose.x() > 2.14 && pose.x() < 2.16)
{
cap = Capability(CYLINDER_2, 90.0, 90.0, 89.0);
}
else
{
return false;
}
}
else
{
return false;
}
octomath::Vector3 unitVector(1.0, 0.0, 0.0);
octomath::Vector3 rotatedVector = pose.rot().rotate(unitVector);
double phi = atan2(rotatedVector.y(), rotatedVector.x()) * 180.0 / M_PI;
double theta = acos(rotatedVector.z()) * 180.0 / M_PI;
return cap.isDirectionPossible(phi, theta);
}
