void CQTOpenGLCamera::Rotate(const QPoint& c_delta) {
m_sSettings[m_unActiveSettings]
.RotateLeftRight2(CRadians(-m_sSettings[m_unActiveSettings].RotationSensitivity * c_delta.x()));
m_sSettings[m_unActiveSettings]
.RotateUpDown(CRadians(m_sSettings[m_unActiveSettings].RotationSensitivity * c_delta.y()));
m_sSettings[m_unActiveSettings]
.Target = m_sSettings[m_unActiveSettings].Position;
m_sSettings[m_unActiveSettings]
.Target += m_sSettings[m_unActiveSettings].Forward;
}
namespace argos {
/****************************************/
/****************************************/
const CRange<CRadians> CCI_FootBotTurretEncoderSensor::ANGULAR_RANGE(CRadians(-ARGOS_PI), CRadians(ARGOS_PI));
/****************************************/
/****************************************/
#ifdef ARGOS_WITH_LUA
void CCI_FootBotTurretEncoderSensor::CreateLuaState(lua_State* pt_lua_state) {
CLuaUtility::OpenRobotStateTable(pt_lua_state, "turret");
CLuaUtility::AddToTable(pt_lua_state, "rotation", m_cRotation);
CLuaUtility::CloseRobotStateTable(pt_lua_state);
}
#endif
/****************************************/
/****************************************/
#ifdef ARGOS_WITH_LUA
void CCI_FootBotTurretEncoderSensor::ReadingsToLuaState(lua_State* pt_lua_state) {
lua_getfield(pt_lua_state, -1, "turret");
lua_pushnumber(pt_lua_state, m_cRotation.GetValue());
lua_setfield(pt_lua_state, -2, "rotation");
lua_pop(pt_lua_state, 1);
}
#endif
/****************************************/
/****************************************/
}
void CFootBotDistanceScannerEquippedEntity::Update() {
if(m_unMode == MODE_SPEED_CONTROL &&
m_fRotationSpeed != 0.0f) {
m_cRotation += CRadians(m_fRotationSpeed *
CPhysicsEngine::GetSimulationClockTick());
m_cRotation.UnsignedNormalize();
}
}
void CFootBotUN::initOdometry() {
position = CVector2(0, 0);
angle = CRadians(0);
velocity = CVector2(0, 0);
axisLength = encoderSensor->GetReading().WheelAxisLength * 0.01;
printf("INIT axis length %.3f", axisLength);
}
/*
* The stack must have one value:
* 1. yaw angle (in radians)
*/
int LuaSetRelativeYaw(lua_State* pt_lua_state) {
/* Check parameters */
if(lua_gettop(pt_lua_state) != 1) {
return luaL_error(pt_lua_state, "robot.quadrotor.set_relative_yaw() expects 1 argument");
}
luaL_checktype(pt_lua_state, 1, LUA_TNUMBER);
/* Perform action */
CLuaUtility::GetDeviceInstance<CCI_QuadRotorPositionActuator>(pt_lua_state, "quadrotor")->
SetRelativeYaw(CRadians(lua_tonumber(pt_lua_state, 1) / 180.0f * CRadians::PI));
return 0;
}
int LuaTurretSetRotation(lua_State* pt_lua_state) {
/* Check parameters */
if(lua_gettop(pt_lua_state) != 1) {
return luaL_error(pt_lua_state, "robot.turret.set_rotation() expects 1 argument");
}
luaL_checktype(pt_lua_state, 1, LUA_TNUMBER);
/* Perform action */
CLuaUtility::GetDeviceInstance<CCI_FootBotTurretActuator>(pt_lua_state, "turret")->
SetRotation(CRadians(lua_tonumber(pt_lua_state, 1)));
return 0;
}
static int BuzzGoToP(buzzvm_t vm) {
/* Push the vector components */
buzzvm_lload(vm, 1);
buzzvm_lload(vm, 2);
/* Create a new vector with that */
buzzobj_t tLinSpeed = buzzvm_stack_at(vm, 2);
buzzobj_t tAngSpeed = buzzvm_stack_at(vm, 1);
Real fLinSpeed = 0.0, fAngSpeed = 0.0;
if(tLinSpeed->o.type == BUZZTYPE_INT) fLinSpeed = tLinSpeed->i.value;
else if(tLinSpeed->o.type == BUZZTYPE_FLOAT) fLinSpeed = tLinSpeed->f.value;
else {
buzzvm_seterror(vm,
BUZZVM_ERROR_TYPE,
"gotop(linspeed,angspeed): expected %s, got %s in first argument",
buzztype_desc[BUZZTYPE_FLOAT],
buzztype_desc[tLinSpeed->o.type]
);
return vm->state;
}
if(tAngSpeed->o.type == BUZZTYPE_INT) fAngSpeed = tAngSpeed->i.value;
else if(tAngSpeed->o.type == BUZZTYPE_FLOAT) fAngSpeed = tAngSpeed->f.value;
else {
buzzvm_seterror(vm,
BUZZVM_ERROR_TYPE,
"gotop(linspeed,angspeed): expected %s, got %s in second argument",
buzztype_desc[BUZZTYPE_FLOAT],
buzztype_desc[tAngSpeed->o.type]
);
return vm->state;
}
CVector2 cDir(fLinSpeed, CRadians(fAngSpeed));
/* Get pointer to the controller */
buzzvm_pushs(vm, buzzvm_string_register(vm, "controller", 1));
buzzvm_gload(vm);
/* Call function */
reinterpret_cast<CBuzzControllerFootBot*>(buzzvm_stack_at(vm, 1)->u.value)->SetWheelSpeedsFromVector(cDir);
return buzzvm_ret0(vm);
}
namespace argos {
/****************************************/
/****************************************/
static const Real FOOTBOT_RADIUS = 0.085036758f;
static const Real FOOTBOT_HEIGHT = 0.146899733f;
static const Real FOOTBOT_LED_RING_RADIUS = FOOTBOT_RADIUS + 0.005;
static const Real FOOTBOT_INTERWHEEL_DISTANCE = 0.14f;
static const Real FOOTBOT_HALF_INTERWHEEL_DISTANCE = FOOTBOT_INTERWHEEL_DISTANCE * 0.5f;
static const Real FOOTBOT_LED_RING_ELEVATION = 0.085f;
static const Real FOOTBOT_BEACON_ELEVATION = 0.174249733f;
static const Real FOOTBOT_GRIPPER_ELEVATION = FOOTBOT_LED_RING_ELEVATION;
/* We can't use CRadians::PI and the likes here because of the 'static initialization order fiasco' */
static const CRadians FOOTBOT_LED_ANGLE_SLICE = CRadians(3.14159265358979323846264338327950288 / 6.0);
static const CRadians FOOTBOT_HALF_LED_ANGLE_SLICE = FOOTBOT_LED_ANGLE_SLICE * 0.5f;
/****************************************/
/****************************************/
class CFootBotEmbodiedEntity : public CEmbodiedEntity {
public:
CFootBotEmbodiedEntity(CFootBotEntity* pc_parent) :
CEmbodiedEntity(pc_parent) {
m_cHalfSize.SetX(FOOTBOT_RADIUS);
m_cHalfSize.SetY(FOOTBOT_RADIUS);
m_cHalfSize.SetZ(FOOTBOT_HEIGHT * 0.5f);
}
protected:
virtual void CalculateBoundingBox() {
m_cCenterPos = GetPosition();
m_cCenterPos.SetZ(m_cCenterPos.GetZ() + m_cHalfSize.GetZ());
m_cOrientationMatrix.FromQuaternion(GetOrientation());
CalculateBoundingBoxFromHalfSize(GetBoundingBox(),
m_cHalfSize,
m_cCenterPos,
m_cOrientationMatrix);
}
private:
CVector3 m_cHalfSize;
CVector3 m_cCenterPos;
CMatrix3x3 m_cOrientationMatrix;
};
/****************************************/
/****************************************/
CFootBotEntity::CFootBotEntity() :
CComposableEntity(NULL),
m_pcEmbodiedEntity(new CFootBotEmbodiedEntity(this)),
m_pcControllableEntity(new CControllableEntity(this)),
m_pcWheeledEntity(new CWheeledEntity<2>(this)),
m_pcLEDEquippedEntity(new CLedEquippedEntity(this)),
m_pcGripperEquippedEntity(new CGripperEquippedEntity(this)),
m_pcDistanceScannerEquippedEntity(new CDistanceScannerEquippedEntity(this)),
m_pcRABEquippedEntity(new CRABEquippedEntity<10>(this)),
m_pcWiFiEquippedEntity(new CWiFiEquippedEntity(this)),
m_fTurretRotationSpeed(0.0f),
m_unTurretMode(0) {
/* Left wheel position */
m_pcWheeledEntity->SetWheelPosition(0, CVector3(0.0f, FOOTBOT_HALF_INTERWHEEL_DISTANCE, 0.0f));
/* Right wheel position */
m_pcWheeledEntity->SetWheelPosition(1, CVector3(0.0f, -FOOTBOT_HALF_INTERWHEEL_DISTANCE, 0.0f));
/* Add LEDs [0-11] and beacon [12] */
for(UInt32 i = 0; i < 13; ++i) {
m_pcLEDEquippedEntity->AddLed(CVector3());
}
/* Gripper position */
m_pcGripperEquippedEntity->SetPosition(CVector3(FOOTBOT_RADIUS, 0.0f, FOOTBOT_GRIPPER_ELEVATION));
}
/****************************************/
/****************************************/
CFootBotEntity::~CFootBotEntity() {
delete m_pcEmbodiedEntity;
delete m_pcControllableEntity;
delete m_pcWheeledEntity;
delete m_pcLEDEquippedEntity;
delete m_pcGripperEquippedEntity;
delete m_pcDistanceScannerEquippedEntity;
delete m_pcRABEquippedEntity;
delete m_pcWiFiEquippedEntity;
}
/****************************************/
/****************************************/
void CFootBotEntity::Init(TConfigurationNode& t_tree) {
try {
/* Init parent */
CEntity::Init(t_tree);
/* Init components */
m_pcEmbodiedEntity->Init(t_tree);
m_pcControllableEntity->Init(t_tree);
m_pcWheeledEntity->Init(t_tree);
m_pcLEDEquippedEntity->Init(t_tree);
m_pcGripperEquippedEntity->Init(t_tree);
m_pcDistanceScannerEquippedEntity->Init(t_tree);
m_pcRABEquippedEntity->Init(t_tree);
m_pcWiFiEquippedEntity->Init(t_tree);
UpdateComponents();
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Failed to initialize entity \"" << GetId() << "\".", ex);
}
}
/****************************************/
/****************************************/
void CFootBotEntity::Reset() {
/* Reset all components */
m_pcEmbodiedEntity->Reset();
m_pcControllableEntity->Reset();
m_pcWheeledEntity->Reset();
m_pcLEDEquippedEntity->Reset();
m_pcGripperEquippedEntity->Reset();
m_pcDistanceScannerEquippedEntity->Reset();
m_pcRABEquippedEntity->Reset();
m_pcWiFiEquippedEntity->Reset();
m_cTurretRotation = CRadians::ZERO;
m_fTurretRotationSpeed = 0.0f;
m_unTurretMode = 0;
/* Update components */
UpdateComponents();
}
/****************************************/
/****************************************/
void CFootBotEntity::Destroy() {
m_pcEmbodiedEntity->Destroy();
m_pcControllableEntity->Destroy();
m_pcWheeledEntity->Destroy();
m_pcLEDEquippedEntity->Destroy();
m_pcGripperEquippedEntity->Destroy();
m_pcDistanceScannerEquippedEntity->Destroy();
m_pcRABEquippedEntity->Destroy();
m_pcWiFiEquippedEntity->Destroy();
m_cTurretRotation = CRadians::ZERO;
m_fTurretRotationSpeed = 0.0f;
m_unTurretMode = 0;
}
/****************************************/
/****************************************/
CEntity& CFootBotEntity::GetComponent(const std::string& str_component) {
if(str_component == "embodied_entity") {
return *m_pcEmbodiedEntity;
}
else if(str_component == "controllable_entity") {
return *m_pcControllableEntity;
}
else if(str_component == "wheeled_entity<2>") {
return *m_pcWheeledEntity;
}
else if(str_component == "led_equipped_entity") {
return *m_pcLEDEquippedEntity;
}
else if(str_component == "gripper_equipped_entity") {
return *m_pcGripperEquippedEntity;
}
else if(str_component == "distance_scanner_equipped_entity") {
return *m_pcDistanceScannerEquippedEntity;
}
else if(str_component == "rab_equipped_entity<10>") {
return *m_pcRABEquippedEntity;
}
else if(str_component == "wifi_equipped_entity") {
return *m_pcWiFiEquippedEntity;
}
else {
THROW_ARGOSEXCEPTION("A foot-bot does not have a component of type \"" << str_component << "\"");
}
}
/****************************************/
/****************************************/
bool CFootBotEntity::HasComponent(const std::string& str_component) {
return (str_component == "embodied_entity" ||
str_component == "controllable_entity" ||
str_component == "wheeled_entity<2>" ||
str_component == "led_equipped_entity" ||
str_component == "gripper_equipped_entity" ||
str_component == "distance_scanner_equipped_entity" ||
str_component == "rab_equipped_entity<10>" ||
str_component == "wifi_equipped_entity");
}
/****************************************/
/****************************************/
void CFootBotEntity::UpdateComponents() {
SetLedPosition();
m_pcDistanceScannerEquippedEntity->UpdateRotation();
m_pcEmbodiedEntity->UpdateBoundingBox();
m_pcRABEquippedEntity->SetPosition(m_pcEmbodiedEntity->GetPosition());
}
/****************************************/
/****************************************/
#define SET_RING_LED_POSITION(IDX) \
cLEDPosition.Set(FOOTBOT_LED_RING_RADIUS, 0.0f, FOOTBOT_LED_RING_ELEVATION); \
cLEDAngle = cLEDAnglePhase; \
cLEDAngle += FOOTBOT_LED_ANGLE_SLICE * IDX; \
cLEDPosition.RotateZ(cLEDAngle); \
cLEDPosition.Rotate(m_pcEmbodiedEntity->GetOrientation()); \
cLEDPosition += cEntityPosition; \
m_pcLEDEquippedEntity->SetLedPosition(IDX, cLEDPosition);
void CFootBotEntity::SetLedPosition() {
/* Set LED positions */
const CVector3& cEntityPosition = GetEmbodiedEntity().GetPosition();
CVector3 cLEDPosition;
CRadians cLEDAnglePhase = FOOTBOT_HALF_LED_ANGLE_SLICE + m_cTurretRotation;
CRadians cLEDAngle;
SET_RING_LED_POSITION(0);
SET_RING_LED_POSITION(1);
SET_RING_LED_POSITION(2);
SET_RING_LED_POSITION(3);
SET_RING_LED_POSITION(4);
SET_RING_LED_POSITION(5);
SET_RING_LED_POSITION(6);
SET_RING_LED_POSITION(7);
SET_RING_LED_POSITION(8);
SET_RING_LED_POSITION(9);
SET_RING_LED_POSITION(10);
SET_RING_LED_POSITION(11);
/* Set beacon position */
cLEDPosition.Set(0.0f, 0.0f, FOOTBOT_BEACON_ELEVATION);
cLEDPosition.Rotate(m_pcEmbodiedEntity->GetOrientation());
cLEDPosition += cEntityPosition;
m_pcLEDEquippedEntity->SetLedPosition(12, cLEDPosition);
}
/****************************************/
/****************************************/
REGISTER_ENTITY(CFootBotEntity,
"foot-bot",
"The foot-bot robot, developed in the Swarmanoid project.",
"Carlo Pinciroli [[email protected]]",
"The foot-bot is a wheeled robot developed in the Swarmanoid Project. It is a\n"
"modular robot with a rich set of sensors and actuators. For more information,\n"
"refer to the dedicated web page\n"
"(http://www.swarmanoid.org/swarmanoid_hardware.php).\n\n"
"REQUIRED XML CONFIGURATION\n\n"
" <arena ...>\n"
" ...\n"
" <foot-bot id=\"fb0\"\n"
" position=\"0.4,2.3,0.25\"\n"
" orientation=\"45,90,0\"\n"
" controller=\"mycntrl\" />\n"
" ...\n"
" </arena>\n\n"
"The 'id' attribute is necessary and must be unique among the entities. If two\n"
"entities share the same id, initialization aborts.\n"
"The 'position' attribute specifies the position of the bottom point of the\n"
"foot-bot in the arena. When the robot is untranslated and unrotated, the\n"
"bottom point is in the origin and it is defined as the middle point between\n"
"the two wheels on the XY plane and the lowest point of the robot on the Z\n"
"axis, that is the point where the wheels touch the floor. The attribute values\n"
"are in the X,Y,Z order.\n"
"The 'orientation' attribute specifies the orientation of the foot-bot. All\n"
"rotations are performed with respect to the bottom point. The order of the\n"
"angles is Z,Y,X, which means that the first number corresponds to the rotation\n"
"around the Z axis, the second around Y and the last around X. This reflects\n"
"the internal convention used in ARGoS, in which rotations are performed in\n"
"that order. Angles are expressed in degrees. When the robot is unrotated, it\n"
"is oriented along the X axis.\n"
"The 'controller' attribute is used to assign a controller to the foot-bot. The\n"
"value of the attribute must be set to the id of a previously defined\n"
"controller. Controllers are defined in the <controllers> XML subtree.\n\n"
"OPTIONAL XML CONFIGURATION\n\n"
"None for the time being.\n",
"Under development"
);
}
CFootBotForaging::SDiffusionParams::SDiffusionParams() :
GoStraightAngleRange(CRadians(-1.0f), CRadians(1.0f)) {}
namespace argos {
/****************************************/
/****************************************/
static const Real BODY_RADIUS = 0.085036758f;
static const Real BODY_HEIGHT = 0.146899733f;
static const Real LED_RING_RADIUS = BODY_RADIUS + 0.005;
static const Real INTERWHEEL_DISTANCE = 0.14f;
static const Real HALF_INTERWHEEL_DISTANCE = INTERWHEEL_DISTANCE * 0.5f;
static const Real WHEEL_RADIUS = 0.029112741f;
static const Real PROXIMITY_SENSOR_RING_ELEVATION = 0.06f;
static const Real PROXIMITY_SENSOR_RING_RADIUS = BODY_RADIUS;
static const CRadians PROXIMITY_SENSOR_RING_START_ANGLE = CRadians((ARGOS_PI / 12.0f) * 0.5f);
static const Real PROXIMITY_SENSOR_RING_RANGE = 0.1f;
static const Real LED_RING_ELEVATION = 0.085f;
static const Real RAB_ELEVATION = 0.1f;
static const Real BEACON_ELEVATION = 0.174249733f;
static const Real GRIPPER_ELEVATION = LED_RING_ELEVATION;
static const CRadians LED_ANGLE_SLICE = CRadians(ARGOS_PI / 6.0);
static const CRadians HALF_LED_ANGLE_SLICE = LED_ANGLE_SLICE * 0.5f;
static const Real OMNIDIRECTIONAL_CAMERA_ELEVATION = 0.288699733f;
/****************************************/
/****************************************/
CFootBotEntity::CFootBotEntity() :
CComposableEntity(NULL),
m_pcControllableEntity(NULL),
m_pcDistanceScannerEquippedEntity(NULL),
m_pcTurretEntity(NULL),
m_pcEmbodiedEntity(NULL),
m_pcGripperEquippedEntity(NULL),
m_pcGroundSensorEquippedEntity(NULL),
m_pcLEDEquippedEntity(NULL),
m_pcLightSensorEquippedEntity(NULL),
m_pcOmnidirectionalCameraEquippedEntity(NULL),
m_pcPerspectiveCameraEquippedEntity(NULL),
m_pcProximitySensorEquippedEntity(NULL),
m_pcRABEquippedEntity(NULL),
m_pcWheeledEntity(NULL),
m_pcWiFiEquippedEntity(NULL) {
}
/****************************************/
/****************************************/
CFootBotEntity::CFootBotEntity(const std::string& str_id,
const std::string& str_controller_id,
const CVector3& c_position,
const CQuaternion& c_orientation,
Real f_rab_range,
size_t un_rab_data_size,
const CRadians& c_omnicam_aperture,
bool b_perspcam_front,
const CRadians& c_perspcam_aperture,
Real f_perspcam_focal_length,
Real f_perspcam_range) :
CComposableEntity(NULL, str_id),
m_pcControllableEntity(NULL),
m_pcDistanceScannerEquippedEntity(NULL),
m_pcTurretEntity(NULL),
m_pcEmbodiedEntity(NULL),
m_pcGripperEquippedEntity(NULL),
m_pcGroundSensorEquippedEntity(NULL),
m_pcLEDEquippedEntity(NULL),
m_pcLightSensorEquippedEntity(NULL),
m_pcOmnidirectionalCameraEquippedEntity(NULL),
m_pcPerspectiveCameraEquippedEntity(NULL),
m_pcProximitySensorEquippedEntity(NULL),
m_pcRABEquippedEntity(NULL),
m_pcWheeledEntity(NULL),
m_pcWiFiEquippedEntity(NULL) {
try {
/*
* Create and init components
*/
/*
* Embodied entity
* Better to put this first, because many other entities need this one
*/
m_pcEmbodiedEntity = new CEmbodiedEntity(this, "body_0", c_position, c_orientation);
AddComponent(*m_pcEmbodiedEntity);
SAnchor& cTurretAnchor = m_pcEmbodiedEntity->AddAnchor("turret");
CQuaternion cPerspCamOrient(b_perspcam_front ? CRadians::ZERO : -CRadians::PI_OVER_TWO,
CVector3::Y);
SAnchor& cPerspCamAnchor = m_pcEmbodiedEntity->AddAnchor("perspective_camera",
CVector3(BODY_RADIUS, 0.0, BEACON_ELEVATION),
cPerspCamOrient);
/* Wheeled entity and wheel positions (left, right) */
m_pcWheeledEntity = new CWheeledEntity(this, "wheels_0", 2);
AddComponent(*m_pcWheeledEntity);
m_pcWheeledEntity->SetWheel(0, CVector3(0.0f, HALF_INTERWHEEL_DISTANCE, 0.0f), WHEEL_RADIUS);
m_pcWheeledEntity->SetWheel(1, CVector3(0.0f, -HALF_INTERWHEEL_DISTANCE, 0.0f), WHEEL_RADIUS);
/* LED equipped entity, with LEDs [0-11] and beacon [12] */
m_pcLEDEquippedEntity = new CLEDEquippedEntity(this, "leds_0");
AddComponent(*m_pcLEDEquippedEntity);
m_pcLEDEquippedEntity->AddLEDRing(
CVector3(0.0f, 0.0f, LED_RING_ELEVATION),
LED_RING_RADIUS,
HALF_LED_ANGLE_SLICE,
12,
cTurretAnchor);
m_pcLEDEquippedEntity->AddLED(
CVector3(0.0f, 0.0f, BEACON_ELEVATION),
cTurretAnchor);
/* Proximity sensor equipped entity */
m_pcProximitySensorEquippedEntity =
new CProximitySensorEquippedEntity(this, "proximity_0");
AddComponent(*m_pcProximitySensorEquippedEntity);
m_pcProximitySensorEquippedEntity->AddSensorRing(
CVector3(0.0f, 0.0f, PROXIMITY_SENSOR_RING_ELEVATION),
PROXIMITY_SENSOR_RING_RADIUS,
PROXIMITY_SENSOR_RING_START_ANGLE,
PROXIMITY_SENSOR_RING_RANGE,
24,
m_pcEmbodiedEntity->GetOriginAnchor());
/* Light sensor equipped entity */
m_pcLightSensorEquippedEntity =
new CLightSensorEquippedEntity(this, "light_0");
AddComponent(*m_pcLightSensorEquippedEntity);
m_pcLightSensorEquippedEntity->AddSensorRing(
CVector3(0.0f, 0.0f, PROXIMITY_SENSOR_RING_ELEVATION),
PROXIMITY_SENSOR_RING_RADIUS,
PROXIMITY_SENSOR_RING_START_ANGLE,
PROXIMITY_SENSOR_RING_RANGE,
24,
m_pcEmbodiedEntity->GetOriginAnchor());
/* Gripper equipped entity */
m_pcGripperEquippedEntity =
new CGripperEquippedEntity(this,
"gripper_0",
CVector3(BODY_RADIUS, 0.0f, GRIPPER_ELEVATION),
CVector3::X);
AddComponent(*m_pcGripperEquippedEntity);
/* Ground sensor equipped entity */
m_pcGroundSensorEquippedEntity =
new CGroundSensorEquippedEntity(this, "ground_0");
AddComponent(*m_pcGroundSensorEquippedEntity);
m_pcGroundSensorEquippedEntity->AddSensor(CVector2(0.063, 0.0116),
CGroundSensorEquippedEntity::TYPE_GRAYSCALE,
m_pcEmbodiedEntity->GetOriginAnchor());
m_pcGroundSensorEquippedEntity->AddSensor(CVector2(-0.063, 0.0116),
CGroundSensorEquippedEntity::TYPE_GRAYSCALE,
m_pcEmbodiedEntity->GetOriginAnchor());
m_pcGroundSensorEquippedEntity->AddSensor(CVector2(-0.063, -0.0116),
CGroundSensorEquippedEntity::TYPE_GRAYSCALE,
m_pcEmbodiedEntity->GetOriginAnchor());
m_pcGroundSensorEquippedEntity->AddSensor(CVector2(0.063, -0.0116),
CGroundSensorEquippedEntity::TYPE_GRAYSCALE,
m_pcEmbodiedEntity->GetOriginAnchor());
m_pcGroundSensorEquippedEntity->AddSensor(CVector2(0.08, 0.0),
CGroundSensorEquippedEntity::TYPE_BLACK_WHITE,
m_pcEmbodiedEntity->GetOriginAnchor());
m_pcGroundSensorEquippedEntity->AddSensor(CVector2(0.042, 0.065),
CGroundSensorEquippedEntity::TYPE_BLACK_WHITE,
m_pcEmbodiedEntity->GetOriginAnchor());
m_pcGroundSensorEquippedEntity->AddSensor(CVector2(0.0, 0.08),
CGroundSensorEquippedEntity::TYPE_BLACK_WHITE,
m_pcEmbodiedEntity->GetOriginAnchor());
m_pcGroundSensorEquippedEntity->AddSensor(CVector2(-0.042, 0.065),
CGroundSensorEquippedEntity::TYPE_BLACK_WHITE,
m_pcEmbodiedEntity->GetOriginAnchor());
m_pcGroundSensorEquippedEntity->AddSensor(CVector2(-0.08, 0.0),
CGroundSensorEquippedEntity::TYPE_BLACK_WHITE,
m_pcEmbodiedEntity->GetOriginAnchor());
m_pcGroundSensorEquippedEntity->AddSensor(CVector2(-0.042, -0.065),
CGroundSensorEquippedEntity::TYPE_BLACK_WHITE,
m_pcEmbodiedEntity->GetOriginAnchor());
m_pcGroundSensorEquippedEntity->AddSensor(CVector2(0.0, -0.08),
CGroundSensorEquippedEntity::TYPE_BLACK_WHITE,
m_pcEmbodiedEntity->GetOriginAnchor());
m_pcGroundSensorEquippedEntity->AddSensor(CVector2(0.042, -0.065),
CGroundSensorEquippedEntity::TYPE_BLACK_WHITE,
m_pcEmbodiedEntity->GetOriginAnchor());
/* Distance scanner */
m_pcDistanceScannerEquippedEntity =
new CFootBotDistanceScannerEquippedEntity(this, "distance_scanner_0");
AddComponent(*m_pcDistanceScannerEquippedEntity);
/* RAB equipped entity */
m_pcRABEquippedEntity =
new CRABEquippedEntity(this,
"rab_0",
un_rab_data_size,
f_rab_range,
m_pcEmbodiedEntity->GetOriginAnchor(),
*m_pcEmbodiedEntity,
CVector3(0.0f, 0.0f, RAB_ELEVATION));
AddComponent(*m_pcRABEquippedEntity);
/* Omnidirectional camera equipped entity */
m_pcOmnidirectionalCameraEquippedEntity =
new COmnidirectionalCameraEquippedEntity(this,
"omnidirectional_camera_0",
c_omnicam_aperture,
CVector3(0.0f,
0.0f,
OMNIDIRECTIONAL_CAMERA_ELEVATION));
AddComponent(*m_pcOmnidirectionalCameraEquippedEntity);
/* Perspective camera equipped entity */
m_pcPerspectiveCameraEquippedEntity =
new CPerspectiveCameraEquippedEntity(this,
"perspective_camera_0",
c_perspcam_aperture,
f_perspcam_focal_length,
f_perspcam_range,
640, 480,
cPerspCamAnchor);
AddComponent(*m_pcPerspectiveCameraEquippedEntity);
/* Turret equipped entity */
m_pcTurretEntity = new CFootBotTurretEntity(this, "turret_0", cTurretAnchor);
AddComponent(*m_pcTurretEntity);
/* WiFi equipped entity */
m_pcWiFiEquippedEntity = new CWiFiEquippedEntity(this, "wifi_0");
AddComponent(*m_pcWiFiEquippedEntity);
/* Controllable entity
It must be the last one, for actuators/sensors to link to composing entities correctly */
m_pcControllableEntity = new CControllableEntity(this, "controller_0");
AddComponent(*m_pcControllableEntity);
m_pcControllableEntity->SetController(str_controller_id);
/* Update components */
UpdateComponents();
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Failed to initialize entity \"" << GetId() << "\".", ex);
}
}
/****************************************/
/****************************************/
void CFootBotEntity::Init(TConfigurationNode& t_tree) {
try {
/*
* Init parent
*/
CComposableEntity::Init(t_tree);
/*
* Create and init components
*/
/*
* Embodied entity
* Better to put this first, because many other entities need this one
*/
m_pcEmbodiedEntity = new CEmbodiedEntity(this);
AddComponent(*m_pcEmbodiedEntity);
m_pcEmbodiedEntity->Init(GetNode(t_tree, "body"));
SAnchor& cTurretAnchor = m_pcEmbodiedEntity->AddAnchor("turret");
/* Wheeled entity and wheel positions (left, right) */
m_pcWheeledEntity = new CWheeledEntity(this, "wheels_0", 2);
AddComponent(*m_pcWheeledEntity);
m_pcWheeledEntity->SetWheel(0, CVector3(0.0f, HALF_INTERWHEEL_DISTANCE, 0.0f), WHEEL_RADIUS);
m_pcWheeledEntity->SetWheel(1, CVector3(0.0f, -HALF_INTERWHEEL_DISTANCE, 0.0f), WHEEL_RADIUS);
/* LED equipped entity, with LEDs [0-11] and beacon [12] */
m_pcLEDEquippedEntity = new CLEDEquippedEntity(this, "leds_0");
AddComponent(*m_pcLEDEquippedEntity);
m_pcLEDEquippedEntity->AddLEDRing(
CVector3(0.0f, 0.0f, LED_RING_ELEVATION),
LED_RING_RADIUS,
HALF_LED_ANGLE_SLICE,
12,
cTurretAnchor);
m_pcLEDEquippedEntity->AddLED(
CVector3(0.0f, 0.0f, BEACON_ELEVATION),
cTurretAnchor);
/* Proximity sensor equipped entity */
m_pcProximitySensorEquippedEntity =
new CProximitySensorEquippedEntity(this, "proximity_0");
AddComponent(*m_pcProximitySensorEquippedEntity);
m_pcProximitySensorEquippedEntity->AddSensorRing(
CVector3(0.0f, 0.0f, PROXIMITY_SENSOR_RING_ELEVATION),
PROXIMITY_SENSOR_RING_RADIUS,
PROXIMITY_SENSOR_RING_START_ANGLE,
PROXIMITY_SENSOR_RING_RANGE,
24,
m_pcEmbodiedEntity->GetOriginAnchor());
/* Light sensor equipped entity */
m_pcLightSensorEquippedEntity =
new CLightSensorEquippedEntity(this, "light_0");
AddComponent(*m_pcLightSensorEquippedEntity);
m_pcLightSensorEquippedEntity->AddSensorRing(
CVector3(0.0f, 0.0f, PROXIMITY_SENSOR_RING_ELEVATION),
PROXIMITY_SENSOR_RING_RADIUS,
PROXIMITY_SENSOR_RING_START_ANGLE,
PROXIMITY_SENSOR_RING_RANGE,
24,
m_pcEmbodiedEntity->GetOriginAnchor());
/* Gripper equipped entity */
m_pcGripperEquippedEntity =
new CGripperEquippedEntity(this,
"gripper_0",
CVector3(BODY_RADIUS, 0.0f, GRIPPER_ELEVATION),
CVector3::X);
AddComponent(*m_pcGripperEquippedEntity);
/* Ground sensor equipped entity */
m_pcGroundSensorEquippedEntity =
new CGroundSensorEquippedEntity(this, "ground_0");
AddComponent(*m_pcGroundSensorEquippedEntity);
m_pcGroundSensorEquippedEntity->AddSensor(CVector2(0.063, 0.0116),
CGroundSensorEquippedEntity::TYPE_GRAYSCALE,
m_pcEmbodiedEntity->GetOriginAnchor());
m_pcGroundSensorEquippedEntity->AddSensor(CVector2(-0.063, 0.0116),
CGroundSensorEquippedEntity::TYPE_GRAYSCALE,
m_pcEmbodiedEntity->GetOriginAnchor());
m_pcGroundSensorEquippedEntity->AddSensor(CVector2(-0.063, -0.0116),
CGroundSensorEquippedEntity::TYPE_GRAYSCALE,
m_pcEmbodiedEntity->GetOriginAnchor());
m_pcGroundSensorEquippedEntity->AddSensor(CVector2(0.063, -0.0116),
CGroundSensorEquippedEntity::TYPE_GRAYSCALE,
m_pcEmbodiedEntity->GetOriginAnchor());
m_pcGroundSensorEquippedEntity->AddSensor(CVector2(0.08, 0.0),
CGroundSensorEquippedEntity::TYPE_BLACK_WHITE,
m_pcEmbodiedEntity->GetOriginAnchor());
m_pcGroundSensorEquippedEntity->AddSensor(CVector2(0.042, 0.065),
CGroundSensorEquippedEntity::TYPE_BLACK_WHITE,
m_pcEmbodiedEntity->GetOriginAnchor());
m_pcGroundSensorEquippedEntity->AddSensor(CVector2(0.0, 0.08),
CGroundSensorEquippedEntity::TYPE_BLACK_WHITE,
m_pcEmbodiedEntity->GetOriginAnchor());
m_pcGroundSensorEquippedEntity->AddSensor(CVector2(-0.042, 0.065),
CGroundSensorEquippedEntity::TYPE_BLACK_WHITE,
m_pcEmbodiedEntity->GetOriginAnchor());
m_pcGroundSensorEquippedEntity->AddSensor(CVector2(-0.08, 0.0),
CGroundSensorEquippedEntity::TYPE_BLACK_WHITE,
m_pcEmbodiedEntity->GetOriginAnchor());
m_pcGroundSensorEquippedEntity->AddSensor(CVector2(-0.042, -0.065),
CGroundSensorEquippedEntity::TYPE_BLACK_WHITE,
m_pcEmbodiedEntity->GetOriginAnchor());
m_pcGroundSensorEquippedEntity->AddSensor(CVector2(0.0, -0.08),
CGroundSensorEquippedEntity::TYPE_BLACK_WHITE,
m_pcEmbodiedEntity->GetOriginAnchor());
m_pcGroundSensorEquippedEntity->AddSensor(CVector2(0.042, -0.065),
CGroundSensorEquippedEntity::TYPE_BLACK_WHITE,
m_pcEmbodiedEntity->GetOriginAnchor());
/* Distance scanner */
m_pcDistanceScannerEquippedEntity =
new CFootBotDistanceScannerEquippedEntity(this, "distance_scanner_0");
AddComponent(*m_pcDistanceScannerEquippedEntity);
/* RAB equipped entity */
Real fRange = 3.0f;
GetNodeAttributeOrDefault(t_tree, "rab_range", fRange, fRange);
UInt32 unDataSize = 10;
GetNodeAttributeOrDefault(t_tree, "rab_data_size", unDataSize, unDataSize);
m_pcRABEquippedEntity =
new CRABEquippedEntity(this,
"rab_0",
unDataSize,
fRange,
m_pcEmbodiedEntity->GetOriginAnchor(),
*m_pcEmbodiedEntity,
CVector3(0.0f, 0.0f, RAB_ELEVATION));
AddComponent(*m_pcRABEquippedEntity);
/* Omnidirectional camera equipped entity */
CDegrees cAperture(70.0f);
GetNodeAttributeOrDefault(t_tree, "omnidirectional_camera_aperture", cAperture, cAperture);
m_pcOmnidirectionalCameraEquippedEntity =
new COmnidirectionalCameraEquippedEntity(this,
"omnidirectional_camera_0",
ToRadians(cAperture),
CVector3(0.0f,
0.0f,
OMNIDIRECTIONAL_CAMERA_ELEVATION));
AddComponent(*m_pcOmnidirectionalCameraEquippedEntity);
/* Perspective camera equipped entity */
bool bPerspCamFront = true;
GetNodeAttributeOrDefault(t_tree, "perspective_camera_front", bPerspCamFront, bPerspCamFront);
Real fPerspCamFocalLength = 0.035;
GetNodeAttributeOrDefault(t_tree, "perspective_camera_focal_length", fPerspCamFocalLength, fPerspCamFocalLength);
Real fPerspCamRange = 2.0;
GetNodeAttributeOrDefault(t_tree, "perspective_camera_range", fPerspCamRange, fPerspCamRange);
cAperture.SetValue(30.0f);
GetNodeAttributeOrDefault(t_tree, "perspective_camera_aperture", cAperture, cAperture);
CQuaternion cPerspCamOrient(bPerspCamFront ? CRadians::ZERO : -CRadians::PI_OVER_TWO,
CVector3::Y);
SAnchor& cPerspCamAnchor = m_pcEmbodiedEntity->AddAnchor("perspective_camera",
CVector3(BODY_RADIUS, 0.0, BEACON_ELEVATION),
cPerspCamOrient);
m_pcPerspectiveCameraEquippedEntity =
new CPerspectiveCameraEquippedEntity(this,
"perspective_camera_0",
ToRadians(cAperture),
fPerspCamFocalLength,
fPerspCamRange,
640, 480,
cPerspCamAnchor);
AddComponent(*m_pcPerspectiveCameraEquippedEntity);
/* Turret equipped entity */
m_pcTurretEntity = new CFootBotTurretEntity(this, "turret_0", cTurretAnchor);
AddComponent(*m_pcTurretEntity);
/* WiFi equipped entity */
m_pcWiFiEquippedEntity = new CWiFiEquippedEntity(this, "wifi_0");
AddComponent(*m_pcWiFiEquippedEntity);
/* Controllable entity
It must be the last one, for actuators/sensors to link to composing entities correctly */
m_pcControllableEntity = new CControllableEntity(this);
AddComponent(*m_pcControllableEntity);
m_pcControllableEntity->Init(GetNode(t_tree, "controller"));
/* Update components */
UpdateComponents();
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Failed to initialize entity \"" << GetId() << "\".", ex);
}
}
/****************************************/
/****************************************/
void CFootBotEntity::Reset() {
/* Reset all components */
CComposableEntity::Reset();
/* Update components */
UpdateComponents();
}
/****************************************/
/****************************************/
#define UPDATE(COMPONENT) if(COMPONENT->IsEnabled()) COMPONENT->Update();
void CFootBotEntity::UpdateComponents() {
/* Update only the components that might change */
UPDATE(m_pcDistanceScannerEquippedEntity);
UPDATE(m_pcTurretEntity);
UPDATE(m_pcGripperEquippedEntity);
UPDATE(m_pcRABEquippedEntity);
UPDATE(m_pcLEDEquippedEntity);
}
/****************************************/
/****************************************/
REGISTER_ENTITY(CFootBotEntity,
"foot-bot",
"Carlo Pinciroli [[email protected]]",
"1.0",
"The foot-bot robot, developed in the Swarmanoid project.",
"The foot-bot is a wheeled robot developed in the Swarmanoid Project. It is a\n"
"modular robot with a rich set of sensors and actuators. For more information,\n"
"refer to the dedicated web page\n"
"(http://www.swarmanoid.org/swarmanoid_hardware.php).\n\n"
"REQUIRED XML CONFIGURATION\n\n"
" <arena ...>\n"
" ...\n"
" <foot-bot id=\"fb0\">\n"
" <body position=\"0.4,2.3,0.25\" orientation=\"45,0,0\" />\n"
" <controller config=\"mycntrl\" />\n"
" </foot-bot>\n"
" ...\n"
" </arena>\n\n"
"The 'id' attribute is necessary and must be unique among the entities. If two\n"
"entities share the same id, initialization aborts.\n"
"The 'body/position' attribute specifies the position of the bottom point of the\n"
"foot-bot in the arena. When the robot is untranslated and unrotated, the\n"
"bottom point is in the origin and it is defined as the middle point between\n"
"the two wheels on the XY plane and the lowest point of the robot on the Z\n"
"axis, that is the point where the wheels touch the floor. The attribute values\n"
"are in the X,Y,Z order.\n"
"The 'body/orientation' attribute specifies the orientation of the foot-bot. All\n"
"rotations are performed with respect to the bottom point. The order of the\n"
"angles is Z,Y,X, which means that the first number corresponds to the rotation\n"
"around the Z axis, the second around Y and the last around X. This reflects\n"
"the internal convention used in ARGoS, in which rotations are performed in\n"
"that order. Angles are expressed in degrees. When the robot is unrotated, it\n"
"is oriented along the X axis.\n"
"The 'controller/config' attribute is used to assign a controller to the\n"
"foot-bot. The value of the attribute must be set to the id of a previously\n"
"defined controller. Controllers are defined in the <controllers> XML subtree.\n\n"
"OPTIONAL XML CONFIGURATION\n\n"
"You can set the emission range of the range-and-bearing system. By default, a\n"
"message sent by a foot-bot can be received up to 3m. By using the 'rab_range'\n"
"attribute, you can change it to, i.e., 4m as follows:\n\n"
" <arena ...>\n"
" ...\n"
" <foot-bot id=\"fb0\" rab_range=\"4\">\n"
" <body position=\"0.4,2.3,0.25\" orientation=\"45,0,0\" />\n"
" <controller config=\"mycntrl\" />\n"
" </foot-bot>\n"
" ...\n"
" </arena>\n\n"
"You can also set the data sent at each time step through the range-and-bearing"
"system. By default, a message sent by a foot-bot is 10 bytes long. By using the"
"'rab_data_size' attribute, you can change it to, i.e., 20 bytes as follows:\n\n"
" <arena ...>\n"
" ...\n"
" <foot-bot id=\"fb0\" rab_data_size=\"20\">\n"
" <body position=\"0.4,2.3,0.25\" orientation=\"45,0,0\" />\n"
" <controller config=\"mycntrl\" />\n"
" </foot-bot>\n"
" ...\n"
" </arena>\n\n"
"You can also change the aperture of the omnidirectional camera. The aperture is\n"
"set to 70 degrees by default. The tip of the omnidirectional camera is placed on\n"
"top of the robot (h=0.289), and with an aperture of 70 degrees the range on the\n"
"ground is r=h*tan(aperture)=0.289*tan(70)=0.794m. To change the aperture to 80\n"
"degrees, use the 'omnidirectional_camera_aperture' as follows:\n\n"
" <arena ...>\n"
" ...\n"
" <foot-bot id=\"fb0\" omnidirectional_camera_aperture=\"80\">\n"
" <body position=\"0.4,2.3,0.25\" orientation=\"45,0,0\" />\n"
" <controller config=\"mycntrl\" />\n"
" </foot-bot>\n"
" ...\n"
" </arena>\n\n"
"Finally, you can change the parameters of the perspective camera. You can set\n"
"its direction, aperture, focal length, and range with the attributes\n"
"'perspective_camera_front', 'perspective_camera_aperture',\n"
"'perspective_camera_focal_length', and 'perspective_camera_range', respectively.\n"
"The default values are: 'true' for front direction, 30 degrees for aperture,\n"
"0.035 for focal length, and 2 meters for range. When the direction is set to\n"
"'false', the camera looks up. This can be useful to see the eye-bot LEDs. Check\n"
"the following example:\n\n"
" <arena ...>\n"
" ...\n"
" <foot-bot id=\"fb0\"\n"
" perspective_camera_front=\"false\"\n"
" perspective_camera_aperture=\"45\"\n"
" perspective_camera_focal_length=\"0.07\"\n"
" perspective_camera_range=\"10\">\n"
" <body position=\"0.4,2.3,0.25\" orientation=\"45,0,0\" />\n"
" <controller config=\"mycntrl\" />\n"
" </foot-bot>\n"
" ...\n"
" </arena>\n\n"
,
"Under development"
);
/****************************************/
/****************************************/
REGISTER_STANDARD_SPACE_OPERATIONS_ON_COMPOSABLE(CFootBotEntity);
/****************************************/
/****************************************/
}
namespace argos {
/****************************************/
/****************************************/
static const Real BODY_HEIGHT = 0.566f;
static const Real BODY_RADIUS = 0.25f;
static const Real LED_RING_RADIUS = BODY_RADIUS + 0.005;
static const Real LED_LOWER_RING_ELEVATION = 0.1535f;
static const Real LED_UPPER_RING_ELEVATION = 0.1635f;
static const CRadians LED_ANGLE_SLICE = CRadians(3.14159265358979323846264338327950288 / 8.0);
static const CRadians HALF_LED_ANGLE_SLICE = LED_ANGLE_SLICE * 0.5f;
static const Real PROXIMITY_SENSOR_RING_ELEVATION = LED_UPPER_RING_ELEVATION;
static const Real PROXIMITY_SENSOR_RING_RADIUS = LED_RING_RADIUS;
static const CRadians PROXIMITY_SENSOR_RING_START_ANGLE = CRadians((ARGOS_PI / 12.0f) * 0.5f);
static const Real PROXIMITY_SENSOR_RING_RANGE = 3.0f;
/****************************************/
/****************************************/
CEyeBotEntity::CEyeBotEntity() :
CComposableEntity(NULL),
m_pcControllableEntity(NULL),
m_pcEmbodiedEntity(NULL),
m_pcLEDEquippedEntity(NULL),
m_pcLightSensorEquippedEntity(NULL),
m_pcProximitySensorEquippedEntity(NULL),
m_pcQuadRotorEntity(NULL),
m_pcRABEquippedEntity(NULL) {
}
/****************************************/
/****************************************/
CEyeBotEntity::CEyeBotEntity(const std::string& str_id,
const std::string& str_controller_id,
const CVector3& c_position,
const CQuaternion& c_orientation,
Real f_rab_range) :
CComposableEntity(NULL, str_id),
m_pcControllableEntity(NULL),
m_pcEmbodiedEntity(NULL),
m_pcLEDEquippedEntity(NULL),
m_pcLightSensorEquippedEntity(NULL),
m_pcProximitySensorEquippedEntity(NULL),
m_pcQuadRotorEntity(NULL),
m_pcRABEquippedEntity(NULL) {
try {
/*
* Create and init components
*/
/*
* Embodied entity
* Better to put this first, because many other entities need this one
*/
m_pcEmbodiedEntity = new CEmbodiedEntity(this, "body_0", c_position, c_orientation);
AddComponent(*m_pcEmbodiedEntity);
/* Quadrotor entity */
m_pcQuadRotorEntity = new CQuadRotorEntity(this, "quadrotor_0");
AddComponent(*m_pcQuadRotorEntity);
/* LED equipped entity, with LEDs [0-11] and beacon [12] */
m_pcLEDEquippedEntity = new CLEDEquippedEntity(this, "leds_0");
m_pcLEDEquippedEntity->AddLEDRing(
CVector3(0.0f, 0.0f, LED_LOWER_RING_ELEVATION),
LED_RING_RADIUS,
HALF_LED_ANGLE_SLICE,
16,
m_pcEmbodiedEntity->GetOriginAnchor());
m_pcLEDEquippedEntity->AddLEDRing(
CVector3(0.0f, 0.0f, LED_UPPER_RING_ELEVATION),
LED_RING_RADIUS,
HALF_LED_ANGLE_SLICE,
16,
m_pcEmbodiedEntity->GetOriginAnchor());
CVector3 cLEDPos(LED_RING_RADIUS * 0.7f,
0.0f,
LED_LOWER_RING_ELEVATION - 0.01f);
cLEDPos.RotateZ(3.0f * CRadians::PI_OVER_FOUR);
m_pcLEDEquippedEntity->AddLED(
cLEDPos,
m_pcEmbodiedEntity->GetOriginAnchor());
AddComponent(*m_pcLEDEquippedEntity);
/* Proximity sensor equipped entity */
m_pcProximitySensorEquippedEntity =
new CProximitySensorEquippedEntity(this,
"proximity_0");
AddComponent(*m_pcProximitySensorEquippedEntity);
m_pcProximitySensorEquippedEntity->AddSensorRing(
CVector3(0.0f, 0.0f, PROXIMITY_SENSOR_RING_ELEVATION),
PROXIMITY_SENSOR_RING_RADIUS,
PROXIMITY_SENSOR_RING_START_ANGLE,
PROXIMITY_SENSOR_RING_RANGE,
24,
m_pcEmbodiedEntity->GetOriginAnchor());
/* Light sensor equipped entity */
m_pcLightSensorEquippedEntity =
new CLightSensorEquippedEntity(this,
"light_0");
AddComponent(*m_pcLightSensorEquippedEntity);
m_pcLightSensorEquippedEntity->AddSensorRing(
CVector3(0.0f, 0.0f, PROXIMITY_SENSOR_RING_ELEVATION),
PROXIMITY_SENSOR_RING_RADIUS,
PROXIMITY_SENSOR_RING_START_ANGLE,
PROXIMITY_SENSOR_RING_RANGE,
24,
m_pcEmbodiedEntity->GetOriginAnchor());
/* RAB equipped entity */
m_pcRABEquippedEntity = new CRABEquippedEntity(
this,
"rab_0",
10,
f_rab_range,
m_pcEmbodiedEntity->GetOriginAnchor(),
*m_pcEmbodiedEntity);
AddComponent(*m_pcRABEquippedEntity);
/* Controllable entity
It must be the last one, for actuators/sensors to link to composing entities correctly */
m_pcControllableEntity = new CControllableEntity(this, "controller_0");
AddComponent(*m_pcControllableEntity);
m_pcControllableEntity->SetController(str_controller_id);
/* Update components */
UpdateComponents();
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Failed to initialize entity \"" << GetId() << "\".", ex);
}
}
/****************************************/
/****************************************/
void CEyeBotEntity::Init(TConfigurationNode& t_tree) {
try {
/*
* Init parent
*/
CComposableEntity::Init(t_tree);
/*
* Create and init components
*/
/*
* Embodied entity
* Better to put this first, because many other entities need this one
*/
m_pcEmbodiedEntity = new CEmbodiedEntity(this);
AddComponent(*m_pcEmbodiedEntity);
m_pcEmbodiedEntity->Init(GetNode(t_tree, "body"));
/* Quadrotor entity */
m_pcQuadRotorEntity = new CQuadRotorEntity(this, "quadrotor_0");
AddComponent(*m_pcQuadRotorEntity);
/* LED equipped entity, with LEDs [0-11] and beacon [12] */
m_pcLEDEquippedEntity = new CLEDEquippedEntity(this, "leds_0");
m_pcLEDEquippedEntity->AddLEDRing(
CVector3(0.0f, 0.0f, LED_LOWER_RING_ELEVATION),
LED_RING_RADIUS,
HALF_LED_ANGLE_SLICE,
16,
m_pcEmbodiedEntity->GetOriginAnchor());
m_pcLEDEquippedEntity->AddLEDRing(
CVector3(0.0f, 0.0f, LED_UPPER_RING_ELEVATION),
LED_RING_RADIUS,
HALF_LED_ANGLE_SLICE,
16,
m_pcEmbodiedEntity->GetOriginAnchor());
CVector3 cLEDPos(LED_RING_RADIUS * 0.7f,
0.0f,
LED_LOWER_RING_ELEVATION - 0.01f);
cLEDPos.RotateZ(3.0f * CRadians::PI_OVER_FOUR);
m_pcLEDEquippedEntity->AddLED(
cLEDPos,
m_pcEmbodiedEntity->GetOriginAnchor());
AddComponent(*m_pcLEDEquippedEntity);
/* Proximity sensor equipped entity */
m_pcProximitySensorEquippedEntity =
new CProximitySensorEquippedEntity(this,
"proximity_0");
AddComponent(*m_pcProximitySensorEquippedEntity);
m_pcProximitySensorEquippedEntity->AddSensorRing(
CVector3(0.0f, 0.0f, PROXIMITY_SENSOR_RING_ELEVATION),
PROXIMITY_SENSOR_RING_RADIUS,
PROXIMITY_SENSOR_RING_START_ANGLE,
PROXIMITY_SENSOR_RING_RANGE,
24,
m_pcEmbodiedEntity->GetOriginAnchor());
/* Light sensor equipped entity */
m_pcLightSensorEquippedEntity =
new CLightSensorEquippedEntity(this,
"light_0");
AddComponent(*m_pcLightSensorEquippedEntity);
m_pcLightSensorEquippedEntity->AddSensorRing(
CVector3(0.0f, 0.0f, PROXIMITY_SENSOR_RING_ELEVATION),
PROXIMITY_SENSOR_RING_RADIUS,
PROXIMITY_SENSOR_RING_START_ANGLE,
PROXIMITY_SENSOR_RING_RANGE,
24,
m_pcEmbodiedEntity->GetOriginAnchor());
/* RAB equipped entity */
Real fRange = 3.0f;
GetNodeAttributeOrDefault(t_tree, "rab_range", fRange, fRange);
m_pcRABEquippedEntity = new CRABEquippedEntity(
this,
"rab_0",
10,
fRange,
m_pcEmbodiedEntity->GetOriginAnchor(),
*m_pcEmbodiedEntity);
AddComponent(*m_pcRABEquippedEntity);
/* Controllable entity
It must be the last one, for actuators/sensors to link to composing entities correctly */
m_pcControllableEntity = new CControllableEntity(this);
AddComponent(*m_pcControllableEntity);
m_pcControllableEntity->Init(GetNode(t_tree, "controller"));
/* Update components */
UpdateComponents();
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Failed to initialize entity \"" << GetId() << "\".", ex);
}
}
/****************************************/
/****************************************/
void CEyeBotEntity::Reset() {
/* Reset all components */
CComposableEntity::Reset();
/* Update components */
UpdateComponents();
}
/****************************************/
/****************************************/
#define UPDATE(COMPONENT) if(COMPONENT->IsEnabled()) COMPONENT->Update();
void CEyeBotEntity::UpdateComponents() {
UPDATE(m_pcRABEquippedEntity);
UPDATE(m_pcLEDEquippedEntity);
}
/****************************************/
/****************************************/
REGISTER_ENTITY(CEyeBotEntity,
"eye-bot",
"Carlo Pinciroli [[email protected]]",
"1.0",
"The eye-bot robot, developed in the Swarmanoid project.",
"The eye-bot is a quad-rotor developed in the Swarmanoid Project. It is a\n"
"fully autonomous robot with a rich set of sensors and actuators. For more\n"
"information, refer to the dedicated web page\n"
"(http://www.swarmanoid.org/swarmanoid_hardware.php).\n\n"
"REQUIRED XML CONFIGURATION\n\n"
" <arena ...>\n"
" ...\n"
" <eye-bot id=\"eb0\">\n"
" <body position=\"0.4,2.3,0.25\" orientation=\"45,0,0\" />\n"
" <controller config=\"mycntrl\" />\n"
" </eye-bot>\n"
" ...\n"
" </arena>\n\n"
"The 'id' attribute is necessary and must be unique among the entities. If two\n"
"entities share the same id, initialization aborts.\n"
"The 'body/position' attribute specifies the position of the bottom point of the\n"
"eye-bot in the arena. When the robot is untranslated and unrotated, the\n"
"bottom point is in the origin and it is defined as the middle point between\n"
"the two wheels on the XY plane and the lowest point of the robot on the Z\n"
"axis, that is the point where the wheels touch the floor. The attribute values\n"
"are in the X,Y,Z order.\n"
"The 'body/orientation' attribute specifies the orientation of the eye-bot. All\n"
"rotations are performed with respect to the bottom point. The order of the\n"
"angles is Z,Y,X, which means that the first number corresponds to the rotation\n"
"around the Z axis, the second around Y and the last around X. This reflects\n"
"the internal convention used in ARGoS, in which rotations are performed in\n"
"that order. Angles are expressed in degrees. When the robot is unrotated, it\n"
"is oriented along the X axis.\n"
"The 'controller/config' attribute is used to assign a controller to the\n"
"eye-bot. The value of the attribute must be set to the id of a previously\n"
"defined controller. Controllers are defined in the <controllers> XML subtree.\n\n"
"OPTIONAL XML CONFIGURATION\n\n"
"You can set the emission range of the range-and-bearing system. By default, a\n"
"message sent by an eye-bot can be received up to 3m. By using the 'rab_range'\n"
"attribute, you can change it to, i.e., 4m as follows:\n\n"
" <arena ...>\n"
" ...\n"
" <eye-bot id=\"fb0\" rab_range=\"4\">\n"
" <body position=\"0.4,2.3,0.25\" orientation=\"45,0,0\" />\n"
" <controller config=\"mycntrl\" />\n"
" </eye-bot>\n"
" ...\n"
" </arena>\n\n"
,
"Under development"
);
/****************************************/
/****************************************/
REGISTER_STANDARD_SPACE_OPERATIONS_ON_COMPOSABLE(CEyeBotEntity);
/****************************************/
/****************************************/
}
void CDynamics2DSingleBodyObjectModel::UpdateOriginAnchor(SAnchor& s_anchor) {
s_anchor.Position.SetX(m_ptBody->p.x);
s_anchor.Position.SetY(m_ptBody->p.y);
s_anchor.Orientation.FromAngleAxis(CRadians(m_ptBody->a), CVector3::Z);
}
void CDynamics2DEngine::OrientationPhysicsToSpace(CQuaternion& c_new_orient,
cpBody* pt_body) {
c_new_orient.FromAngleAxis(CRadians(pt_body->a), CVector3::Z);
}
namespace argos {
/****************************************/
/****************************************/
static CRadians SPACING = CRadians(ARGOS_PI / 12.0f);
static CRadians START_ANGLE = SPACING * 0.5f;
/****************************************/
/****************************************/
CCI_FootBotProximitySensor::CCI_FootBotProximitySensor() :
m_tReadings(24) {
for(size_t i = 0; i < 24; ++i) {
m_tReadings[i].Angle = START_ANGLE + i * SPACING;
m_tReadings[i].Angle.SignedNormalize();
}
}
/****************************************/
/****************************************/
const CCI_FootBotProximitySensor::TReadings& CCI_FootBotProximitySensor::GetReadings() const {
return m_tReadings;
}
/****************************************/
/****************************************/
#ifdef ARGOS_WITH_LUA
void CCI_FootBotProximitySensor::CreateLuaState(lua_State* pt_lua_state) {
CLuaUtility::OpenRobotStateTable(pt_lua_state, "proximity");
for(size_t i = 0; i < GetReadings().size(); ++i) {
CLuaUtility::StartTable(pt_lua_state, i+1 );
CLuaUtility::AddToTable(pt_lua_state, "angle", m_tReadings[i].Angle);
CLuaUtility::AddToTable(pt_lua_state, "value", m_tReadings[i].Value);
CLuaUtility::EndTable (pt_lua_state );
}
CLuaUtility::CloseRobotStateTable(pt_lua_state);
}
#endif
/****************************************/
/****************************************/
#ifdef ARGOS_WITH_LUA
void CCI_FootBotProximitySensor::ReadingsToLuaState(lua_State* pt_lua_state) {
lua_getfield(pt_lua_state, -1, "proximity");
for(size_t i = 0; i < GetReadings().size(); ++i) {
lua_pushnumber(pt_lua_state, i+1 );
lua_gettable (pt_lua_state, -2 );
lua_pushnumber(pt_lua_state, m_tReadings[i].Value);
lua_setfield (pt_lua_state, -2, "value" );
lua_pop(pt_lua_state, 1);
}
lua_pop(pt_lua_state, 1);
}
#endif
/****************************************/
/****************************************/
std::ostream& operator<<(std::ostream& c_os,
const CCI_FootBotProximitySensor::SReading& s_reading) {
c_os << "Value=<" << s_reading.Value
<< ">, Angle=<" << s_reading.Angle << ">";
return c_os;
}
/****************************************/
/****************************************/
std::ostream& operator<<(std::ostream& c_os,
const CCI_FootBotProximitySensor::TReadings& t_readings) {
if(! t_readings.empty()) {
c_os << "{ " << t_readings[0].Value << " }";
for(UInt32 i = 1; i < t_readings.size(); ++i) {
c_os << " { " << t_readings[0].Value << " }";
}
c_os << std::endl;
}
return c_os;
}
/****************************************/
/****************************************/
}
namespace argos {
/****************************************/
/****************************************/
static const Real EPUCK_RADIUS = 0.035f;
static const Real EPUCK_HEIGHT = 0.086f;
static const Real EPUCK_LED_RING_RADIUS = EPUCK_RADIUS + 0.007;
static const Real EPUCK_INTERWHEEL_DISTANCE = 0.053f;
static const Real EPUCK_HALF_INTERWHEEL_DISTANCE = EPUCK_INTERWHEEL_DISTANCE * 0.5f;
static const Real EPUCK_LED_RING_ELEVATION = 0.086f;
static const Real EPUCK_CAMERA_ELEVATION = 0.027f;
/* We can't use CRadians::PI and the likes here because of the 'static initialization order fiasco' */
static const CRadians EPUCK_LED_ANGLE_SLICE = CRadians(ARGOS_PI / 4.0);
static const CRadians EPUCK_HALF_LED_ANGLE_SLICE = EPUCK_LED_ANGLE_SLICE * 0.5f;
/****************************************/
/****************************************/
class CEPuckEmbodiedEntity : public CEmbodiedEntity {
public:
CEPuckEmbodiedEntity(CEPuckEntity* pc_parent) :
CEmbodiedEntity(pc_parent) {
m_cHalfSize.SetX(EPUCK_RADIUS);
m_cHalfSize.SetY(EPUCK_RADIUS);
m_cHalfSize.SetZ(EPUCK_HEIGHT * 0.5f);
}
protected:
virtual void CalculateBoundingBox() {
m_cCenterPos = GetPosition();
m_cCenterPos.SetZ(m_cCenterPos.GetZ() + m_cHalfSize.GetZ());
m_cOrientationMatrix.FromQuaternion(GetOrientation());
CalculateBoundingBoxFromHalfSize(GetBoundingBox(),
m_cHalfSize,
m_cCenterPos,
m_cOrientationMatrix);
}
private:
CVector3 m_cHalfSize;
CVector3 m_cCenterPos;
CMatrix3x3 m_cOrientationMatrix;
};
/****************************************/
/****************************************/
CEPuckEntity::CEPuckEntity() :
CComposableEntity(NULL),
m_pcEmbodiedEntity(new CEPuckEmbodiedEntity(this)),
m_pcControllableEntity(new CControllableEntity(this)),
m_pcWheeledEntity(new CWheeledEntity<2>(this)),
m_pcLEDEquippedEntity(new CLedEquippedEntity(this)),
m_pcRABEquippedEntity(new CRABEquippedEntity<2>(this)) {
/* Left wheel position */
m_pcWheeledEntity->SetWheelPosition(0, CVector3(0.0f, EPUCK_HALF_INTERWHEEL_DISTANCE, 0.0f));
/* Right wheel position */
m_pcWheeledEntity->SetWheelPosition(1, CVector3(0.0f, -EPUCK_HALF_INTERWHEEL_DISTANCE, 0.0f));
/* Add LEDs [0-7] */
for(UInt32 i = 0; i < 8; ++i) {
m_pcLEDEquippedEntity->AddLed(CVector3());
}
}
/****************************************/
/****************************************/
CEPuckEntity::~CEPuckEntity() {
delete m_pcEmbodiedEntity;
delete m_pcControllableEntity;
delete m_pcWheeledEntity;
delete m_pcLEDEquippedEntity;
delete m_pcRABEquippedEntity;
}
/****************************************/
/****************************************/
void CEPuckEntity::Init(TConfigurationNode& t_tree) {
try {
/* Init parent */
CEntity::Init(t_tree);
/* Init components */
m_pcEmbodiedEntity->Init(t_tree);
m_pcControllableEntity->Init(t_tree);
m_pcWheeledEntity->Init(t_tree);
m_pcLEDEquippedEntity->Init(t_tree);
m_pcRABEquippedEntity->Init(t_tree);
UpdateComponents();
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Failed to initialize entity \"" << GetId() << "\".", ex);
}
}
/****************************************/
/****************************************/
void CEPuckEntity::Reset() {
/* Reset all components */
m_pcEmbodiedEntity->Reset();
m_pcControllableEntity->Reset();
m_pcWheeledEntity->Reset();
m_pcLEDEquippedEntity->Reset();
m_pcRABEquippedEntity->Reset();
/* Update components */
UpdateComponents();
}
/****************************************/
/****************************************/
void CEPuckEntity::Destroy() {
m_pcEmbodiedEntity->Destroy();
m_pcControllableEntity->Destroy();
m_pcWheeledEntity->Destroy();
m_pcLEDEquippedEntity->Destroy();
m_pcRABEquippedEntity->Destroy();
}
/****************************************/
/****************************************/
CEntity& CEPuckEntity::GetComponent(const std::string& str_component) {
if(str_component == "embodied_entity") {
return *m_pcEmbodiedEntity;
}
else if(str_component == "controllable_entity") {
return *m_pcControllableEntity;
}
else if(str_component == "wheeled_entity<2>") {
return *m_pcWheeledEntity;
}
else if(str_component == "led_equipped_entity") {
return *m_pcLEDEquippedEntity;
}
else if(str_component == "rab_equipped_entity<2>"){
return *m_pcRABEquippedEntity;
}
else {
THROW_ARGOSEXCEPTION("An e-puck does not have a component of type \"" << str_component << "\"");
}
}
/****************************************/
/****************************************/
bool CEPuckEntity::HasComponent(const std::string& str_component) {
return (str_component == "embodied_entity" ||
str_component == "controllable_entity" ||
str_component == "wheeled_entity<2>" ||
str_component == "led_equipped_entity" ||
str_component == "rab_equipped_entity<2>");
}
/****************************************/
/****************************************/
void CEPuckEntity::UpdateComponents() {
SetLedPosition();
m_pcEmbodiedEntity->UpdateBoundingBox();
m_pcRABEquippedEntity->SetPosition(m_pcEmbodiedEntity->GetPosition());
}
/****************************************/
/****************************************/
#define SET_RING_LED_POSITION(IDX) \
cLEDPosition.Set(EPUCK_LED_RING_RADIUS, 0.0f, EPUCK_LED_RING_ELEVATION); \
cLEDAngle = -EPUCK_HALF_LED_ANGLE_SLICE; \
cLEDAngle -= EPUCK_LED_ANGLE_SLICE * IDX; \
cLEDPosition.RotateZ(cLEDAngle); \
cLEDPosition.Rotate(m_pcEmbodiedEntity->GetOrientation()); \
cLEDPosition += cEntityPosition; \
m_pcLEDEquippedEntity->SetLedPosition(IDX, cLEDPosition);
void CEPuckEntity::SetLedPosition() {
/* Set LED positions */
const CVector3& cEntityPosition = GetEmbodiedEntity().GetPosition();
CVector3 cLEDPosition;
CRadians cLEDAngle;
SET_RING_LED_POSITION(0);
SET_RING_LED_POSITION(1);
SET_RING_LED_POSITION(2);
SET_RING_LED_POSITION(3);
SET_RING_LED_POSITION(4);
SET_RING_LED_POSITION(5);
SET_RING_LED_POSITION(6);
SET_RING_LED_POSITION(7);
}
/****************************************/
/****************************************/
REGISTER_ENTITY(CEPuckEntity,
"e-puck",
"The e-puck robot.",
"Carlo Pinciroli [[email protected]]",
"The e-puck is a simple wheeled robot.\n\n"
"REQUIRED XML CONFIGURATION\n\n"
" <arena ...>\n"
" ...\n"
" <e-puck id=\"fb0\"\n"
" position=\"0.4,2.3,0.25\"\n"
" orientation=\"45,90,0\"\n"
" controller=\"mycntrl\" />\n"
" ...\n"
" </arena>\n\n"
"The 'id' attribute is necessary and must be unique among the entities. If two\n"
"entities share the same id, initialization aborts.\n"
"The 'position' attribute specifies the position of the pucktom point of the\n"
"e-puck in the arena. When the robot is untranslated and unrotated, the\n"
"pucktom point is in the origin and it is defined as the middle point between\n"
"the two wheels on the XY plane and the lowest point of the robot on the Z\n"
"axis, that is the point where the wheels touch the floor. The attribute values\n"
"are in the X,Y,Z order.\n"
"The 'orientation' attribute specifies the orientation of the e-puck. All\n"
"rotations are performed with respect to the pucktom point. The order of the\n"
"angles is Z,Y,X, which means that the first number corresponds to the rotation\n"
"around the Z axis, the second around Y and the last around X. This reflects\n"
"the internal convention used in ARGoS, in which rotations are performed in\n"
"that order. Angles are expressed in degrees. When the robot is unrotated, it\n"
"is oriented along the X axis.\n"
"The 'controller' attribute is used to assign a controller to the e-puck. The\n"
"value of the attribute must be set to the id of a previously defined\n"
"controller. Controllers are defined in the <controllers> XML subtree.\n\n"
"OPTIONAL XML CONFIGURATION\n\n"
"None for the time being.\n",
"Under development"
);
}
Real CProprioceptiveFeatureVector::TrackRobotDisplacement(Real step, std::vector<RobotRelativePosData>& displacement_vector)
{
Real displacement = 0.0f;
CRadians delta_orientation = CRadians(m_sRobotData.seconds_per_iterations * ((-m_sSensoryData.f_LeftWheelSpeed_prev + m_sSensoryData.f_RightWheelSpeed_prev)
/ (m_sRobotData.INTERWHEEL_DISTANCE*100.0f)));
/*
* Computing average velocity
*/
if(step < (Real)displacement_vector.size())
{
//vec_RobotRelativePosition[(unsigned)(m_sSensoryData.m_rTime - m_fTimeFirstObserved)]
for (size_t t = 0 ; t < (unsigned)(step); ++t)
{
displacement_vector[t].TimeSinceStart++;
CRadians prev_orientation = displacement_vector[t].NetRotationSinceStart;
Real rX = m_sRobotData.seconds_per_iterations *
((m_sSensoryData.f_LeftWheelSpeed_prev + m_sSensoryData.f_RightWheelSpeed_prev) / 2.0f) * Cos(prev_orientation + delta_orientation / (2.0f));
Real rY = m_sRobotData.seconds_per_iterations *
((m_sSensoryData.f_LeftWheelSpeed_prev + m_sSensoryData.f_RightWheelSpeed_prev) / 2.0f) * Sin(prev_orientation + delta_orientation / (2.0f));
displacement_vector[t].NetTranslationSinceStart += CVector2(rX, rY);
displacement_vector[t].NetRotationSinceStart += delta_orientation;
}
displacement_vector[(unsigned)(step)].TimeSinceStart = 0.0f;
displacement_vector[(unsigned)(step)].NetTranslationSinceStart.Set(0.0f, 0.0f);
displacement_vector[(unsigned)(step)].NetRotationSinceStart.SetValue(0.0f);
}
else
{
for (size_t t = 0 ; t < displacement_vector.size(); ++t)
{
displacement_vector[t].TimeSinceStart++;
CRadians prev_orientation = displacement_vector[t].NetRotationSinceStart;
Real rX = m_sRobotData.seconds_per_iterations *
((m_sSensoryData.f_LeftWheelSpeed_prev + m_sSensoryData.f_RightWheelSpeed_prev) / 2.0f) * Cos(prev_orientation + delta_orientation / (2.0f));
Real rY = m_sRobotData.seconds_per_iterations *
((m_sSensoryData.f_LeftWheelSpeed_prev + m_sSensoryData.f_RightWheelSpeed_prev) / 2.0f) * Sin(prev_orientation + delta_orientation / (2.0f));
displacement_vector[t].NetTranslationSinceStart += CVector2(rX, rY);
displacement_vector[t].NetRotationSinceStart += delta_orientation;
}
size_t t = ((unsigned)(step)%displacement_vector.size());
/*
* Computing average displacement
*/
displacement = (displacement_vector[t].NetTranslationSinceStart).Length();
displacement_vector[t].TimeSinceStart = 0.0f;
displacement_vector[t].NetTranslationSinceStart.Set(0.0f, 0.0f);
displacement_vector[t].NetRotationSinceStart.SetValue(0.0f);
}
return displacement;
}
namespace argos {
/****************************************/
/****************************************/
static CRange<Real> SENSOR_RANGE(0.0f, 1.0f);
static CRadians SENSOR_SPACING = CRadians(ARGOS_PI / 12.0f);
static CRadians SENSOR_HALF_SPACING = SENSOR_SPACING * 0.5;
/****************************************/
/****************************************/
static SInt32 Modulo(SInt32 n_value, SInt32 un_modulo) {
while(n_value < 0) n_value += un_modulo;
while(n_value >= un_modulo) n_value -= un_modulo;
return n_value;
}
static Real ComputeReading(Real f_distance) {
if(f_distance > 2.5f) {
return 0.0f;
}
else {
return ::exp(-f_distance * 2.0f);
}
}
static Real ScaleReading(const CRadians& c_angular_distance) {
if(c_angular_distance > CRadians::PI_OVER_TWO) {
return 0.0f;
}
else {
return (1.0f - 2.0f * c_angular_distance / CRadians::PI);
}
}
/****************************************/
/****************************************/
CFootBotLightRotZOnlySensor::CFootBotLightRotZOnlySensor() :
m_pcEmbodiedEntity(NULL),
m_bShowRays(false),
m_pcRNG(NULL),
m_bAddNoise(false),
m_cSpace(CSimulator::GetInstance().GetSpace()) {}
/****************************************/
/****************************************/
void CFootBotLightRotZOnlySensor::SetRobot(CComposableEntity& c_entity) {
try {
m_pcEmbodiedEntity = &(c_entity.GetComponent<CEmbodiedEntity>("body"));
m_pcControllableEntity = &(c_entity.GetComponent<CControllableEntity>("controller"));
m_pcLightEntity = &(c_entity.GetComponent<CLightSensorEquippedEntity>("light_sensors"));
m_pcLightEntity->Enable();
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Can't set robot for the foot-bot light default sensor", ex);
}
}
/****************************************/
/****************************************/
void CFootBotLightRotZOnlySensor::Init(TConfigurationNode& t_tree) {
try {
/* Show rays? */
GetNodeAttributeOrDefault(t_tree, "show_rays", m_bShowRays, m_bShowRays);
/* Parse noise level */
Real fNoiseLevel = 0.0f;
GetNodeAttributeOrDefault(t_tree, "noise_level", fNoiseLevel, fNoiseLevel);
if(fNoiseLevel < 0.0f) {
THROW_ARGOSEXCEPTION("Can't specify a negative value for the noise level of the light sensor");
}
else if(fNoiseLevel > 0.0f) {
m_bAddNoise = true;
m_cNoiseRange.Set(-fNoiseLevel, fNoiseLevel);
m_pcRNG = CRandom::CreateRNG("argos");
}
m_tReadings.resize(m_pcLightEntity->GetNumSensors());
}
catch(CARGoSException& ex) {
THROW_ARGOSEXCEPTION_NESTED("Initialization error in rot_z_only light sensor", ex);
}
}
/****************************************/
/****************************************/
void CFootBotLightRotZOnlySensor::Update() {
/* Erase readings */
for(size_t i = 0; i < m_tReadings.size(); ++i) {
m_tReadings[i].Value = 0.0f;
}
/* Get foot-bot orientation */
CRadians cTmp1, cTmp2, cOrientationZ;
m_pcEmbodiedEntity->GetOriginAnchor().Orientation.ToEulerAngles(cOrientationZ, cTmp1, cTmp2);
/* Ray used for scanning the environment for obstacles */
CRay3 cOcclusionCheckRay;
cOcclusionCheckRay.SetStart(m_pcEmbodiedEntity->GetOriginAnchor().Position);
CVector3 cRobotToLight;
/* Buffer for the angle of the light wrt to the foot-bot */
CRadians cAngleLightWrtFootbot;
/* Buffers to contain data about the intersection */
SEmbodiedEntityIntersectionItem sIntersection;
/* List of light entities */
CSpace::TMapPerTypePerId::iterator itLights = m_cSpace.GetEntityMapPerTypePerId().find("light");
if (itLights != m_cSpace.GetEntityMapPerTypePerId().end()) {
CSpace::TMapPerType& mapLights = itLights->second;
/*
* 1. go through the list of light entities in the scene
* 2. check if a light is occluded
* 3. if it isn't, distribute the reading across the sensors
* NOTE: the readings are additive
* 4. go through the sensors and clamp their values
*/
for(CSpace::TMapPerType::iterator it = mapLights.begin();
it != mapLights.end();
++it) {
/* Get a reference to the light */
CLightEntity& cLight = *(any_cast<CLightEntity*>(it->second));
/* Consider the light only if it has non zero intensity */
if(cLight.GetIntensity() > 0.0f) {
/* Set the ray end */
cOcclusionCheckRay.SetEnd(cLight.GetPosition());
/* Check occlusion between the foot-bot and the light */
if(! GetClosestEmbodiedEntityIntersectedByRay(sIntersection,
cOcclusionCheckRay,
*m_pcEmbodiedEntity)) {
/* The light is not occluded */
if(m_bShowRays) {
m_pcControllableEntity->AddCheckedRay(false, cOcclusionCheckRay);
}
/* Get the distance between the light and the foot-bot */
cOcclusionCheckRay.ToVector(cRobotToLight);
/*
* Linearly scale the distance with the light intensity
* The greater the intensity, the smaller the distance
*/
cRobotToLight /= cLight.GetIntensity();
/* Get the angle wrt to foot-bot rotation */
cAngleLightWrtFootbot = cRobotToLight.GetZAngle();
cAngleLightWrtFootbot -= cOrientationZ;
/*
* Find closest sensor index to point at which ray hits footbot body
* Rotate whole body by half a sensor spacing (corresponding to placement of first sensor)
* Division says how many sensor spacings there are between first sensor and point at which ray hits footbot body
* Increase magnitude of result of division to ensure correct rounding
*/
Real fIdx = (cAngleLightWrtFootbot - SENSOR_HALF_SPACING) / SENSOR_SPACING;
SInt32 nReadingIdx = (fIdx > 0) ? fIdx + 0.5f : fIdx - 0.5f;
/* Set the actual readings */
Real fReading = cRobotToLight.Length();
/*
* Take 6 readings before closest sensor and 6 readings after - thus we
* process sensors that are with 180 degrees of intersection of light
* ray with robot body
*/
for(SInt32 nIndexOffset = -6; nIndexOffset < 7; ++nIndexOffset) {
UInt32 unIdx = Modulo(nReadingIdx + nIndexOffset, 24);
CRadians cAngularDistanceFromOptimalLightReceptionPoint = Abs((cAngleLightWrtFootbot - m_tReadings[unIdx].Angle).SignedNormalize());
/*
* ComputeReading gives value as if sensor was perfectly in line with
* light ray. We then linearly decrease actual reading from 1 (dist
* 0) to 0 (dist PI/2)
*/
m_tReadings[unIdx].Value += ComputeReading(fReading) * ScaleReading(cAngularDistanceFromOptimalLightReceptionPoint);
}
}
else {
/* The ray is occluded */
if(m_bShowRays) {
m_pcControllableEntity->AddCheckedRay(true, cOcclusionCheckRay);
m_pcControllableEntity->AddIntersectionPoint(cOcclusionCheckRay, sIntersection.TOnRay);
}
}
}
}
/* Apply noise to the sensors */
if(m_bAddNoise) {
for(size_t i = 0; i < 24; ++i) {
m_tReadings[i].Value += m_pcRNG->Uniform(m_cNoiseRange);
}
}
/* Trunc the reading between 0 and 1 */
for(size_t i = 0; i < 24; ++i) {
SENSOR_RANGE.TruncValue(m_tReadings[i].Value);
}
}
else {
/* There are no lights in the environment */
if(m_bAddNoise) {
/* Go through the sensors */
for(UInt32 i = 0; i < m_tReadings.size(); ++i) {
/* Apply noise to the sensor */
m_tReadings[i].Value += m_pcRNG->Uniform(m_cNoiseRange);
/* Trunc the reading between 0 and 1 */
SENSOR_RANGE.TruncValue(m_tReadings[i].Value);
}
}
}
}
/****************************************/
/****************************************/
void CFootBotLightRotZOnlySensor::Reset() {
for(UInt32 i = 0; i < GetReadings().size(); ++i) {
m_tReadings[i].Value = 0.0f;
}
}
/****************************************/
/****************************************/
REGISTER_SENSOR(CFootBotLightRotZOnlySensor,
"footbot_light", "rot_z_only",
"Carlo Pinciroli [[email protected]]",
"1.0",
"The foot-bot light sensor (optimized for 2D).",
"This sensor accesses a set of light sensors. The sensors all return a value\n"
"between 0 and 1, where 0 means nothing within range and 1 means the perceived\n"
"light saturates the sensor. Values between 0 and 1 depend on the distance of\n"
"the perceived light. Each reading R is calculated with R=(I/x)^2, where x is the\n"
"distance between a sensor and the light, and I is the reference intensity of the\n"
"perceived light. The reference intensity corresponds to the minimum distance at\n"
"which the light saturates a sensor. The reference intensity depends on the\n"
"individual light, and it is set with the \"intensity\" attribute of the light\n"
"entity. In case multiple lights are present in the environment, each sensor\n"
"reading is calculated as the sum of the individual readings due to each light.\n"
"In other words, light wave interference is not taken into account. In\n"
"controllers, you must include the ci_light_sensor.h header.\n\n"
"REQUIRED XML CONFIGURATION\n\n"
" <controllers>\n"
" ...\n"
" <my_controller ...>\n"
" ...\n"
" <sensors>\n"
" ...\n"
" <footbot_light implementation=\"rot_z_only\" />\n"
" ...\n"
" </sensors>\n"
" ...\n"
" </my_controller>\n"
" ...\n"
" </controllers>\n\n"
"OPTIONAL XML CONFIGURATION\n\n"
"It is possible to draw the rays shot by the light sensor in the OpenGL\n"
"visualization. This can be useful for sensor debugging but also to understand\n"
"what's wrong in your controller. In OpenGL, the rays are drawn in cyan when\n"
"they are not obstructed and in purple when they are. In case a ray is\n"
"obstructed, a black dot is drawn where the intersection occurred.\n"
"To turn this functionality on, add the attribute \"show_rays\" as in this\n"
"example:\n\n"
" <controllers>\n"
" ...\n"
" <my_controller ...>\n"
" ...\n"
" <sensors>\n"
" ...\n"
" <footbot_light implementation=\"rot_z_only\"\n"
" show_rays=\"true\" />\n"
" ...\n"
" </sensors>\n"
" ...\n"
" </my_controller>\n"
" ...\n"
" </controllers>\n\n"
"It is possible to add uniform noise to the sensors, thus matching the\n"
"characteristics of a real robot better. This can be done with the attribute\n"
"\"noise_level\", whose allowed range is in [-1,1] and is added to the calculated\n"
"reading. The final sensor reading is always normalized in the [0-1] range.\n\n"
" <controllers>\n"
" ...\n"
" <my_controller ...>\n"
" ...\n"
" <sensors>\n"
" ...\n"
" <footbot_light implementation=\"rot_z_only\"\n"
" noise_level=\"0.1\" />\n"
" ...\n"
" </sensors>\n"
" ...\n"
" </my_controller>\n"
" ...\n"
" </controllers>\n\n"
"OPTIONAL XML CONFIGURATION\n\n"
"None.\n",
"Usable"
);
}
