CRABEquippedEntity::CRABEquippedEntity(CComposableEntity* pc_parent,
const std::string& str_id,
size_t un_msg_size,
Real f_range,
const SAnchor& s_anchor,
CEmbodiedEntity& c_entity_body,
const CVector3& c_pos_offset,
const CQuaternion& c_rot_offset) :
CPositionalEntity(pc_parent,
str_id),
m_psAnchor(&s_anchor),
m_cPosOffset(c_pos_offset),
m_cRotOffset(c_rot_offset),
m_cData(un_msg_size),
m_fRange(f_range),
m_pcEntityBody(&c_entity_body) {
Disable();
SetCanBeEnabledIfDisabled(false);
CVector3 cPos = c_pos_offset;
cPos.Rotate(s_anchor.Orientation);
cPos += s_anchor.Position;
SetInitPosition(cPos);
SetPosition(cPos);
SetInitOrientation(s_anchor.Orientation * c_rot_offset);
SetOrientation(GetInitOrientation());
}
void CRABEquippedEntity::Init(TConfigurationNode& t_tree) {
try {
/*
* Init entity.
* Here we explicitly avoid to call CPositionalEntity::Init() because that
* would also initialize position and orientation, which, instead, must
* be calculated from reference entity and offsets.
*/
CEntity::Init(t_tree);
/* Get offsets */
GetNodeAttributeOrDefault(t_tree, "pos_offset", m_cPosOffset, m_cPosOffset);
std::string strRotOffset;
GetNodeAttributeOrDefault(t_tree, "rot_offset", strRotOffset, strRotOffset);
if(strRotOffset != "") {
CDegrees cRotOffsetEuler[3];
ParseValues(strRotOffset, 3, cRotOffsetEuler, ',');
m_cRotOffset.FromEulerAngles(ToRadians(cRotOffsetEuler[0]),
ToRadians(cRotOffsetEuler[1]),
ToRadians(cRotOffsetEuler[2]));
}
/* Parse and look up the anchor */
std::string strAnchorId;
GetNodeAttribute(t_tree, "anchor", strAnchorId);
/*
* NOTE: here we get a reference to the embodied entity
* This line works under the assumption that:
* 1. the RABEquippedEntity has a parent;
* 2. the parent has a child whose id is "body"
* 3. the "body" is an embodied entity
* If any of the above is false, this line will bomb out.
*/
m_pcEntityBody = &GetParent().GetComponent<CEmbodiedEntity>("body");
m_psAnchor = &m_pcEntityBody->GetAnchor(strAnchorId);
/* Get message size */
size_t unMsgSize;
GetNodeAttribute(t_tree, "msg_size", unMsgSize);
m_cData.Resize(unMsgSize);
/* Get transmission range */
GetNodeAttribute(t_tree, "range", m_fRange);
/* Set init position and orientation */
Update();
SetInitPosition(GetPosition());
SetInitOrientation(GetOrientation());
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Error initializing a range and bearing entity \"" << GetId() << "\"", ex);
}
}
CKilobotCommunicationEntity::CKilobotCommunicationEntity(CComposableEntity* pc_parent,
const std::string& str_id,
size_t un_msg_size,
Real f_range,
SAnchor& s_anchor,
CEmbodiedEntity& c_entity_body) :
CPositionalEntity(pc_parent,
str_id),
m_psAnchor(&s_anchor),
m_fTxRange(f_range),
m_pcEntityBody(&c_entity_body),
m_eTxStatus(TX_NONE) {
Disable();
SetInitPosition(s_anchor.Position);
SetPosition(GetInitPosition());
SetInitOrientation(s_anchor.Orientation);
SetOrientation(GetInitOrientation());
}
