Code: Select all
[FATAL] Failed any_cast conversion from argos::CKilobotEntity* to argos::CDirectionalLEDEntity*Code: Select all
#include "trajectory_loop_functions.h"
#include "argos3/core/control_interface/ci_controller.h"
#include "argos3/plugins/simulator/entities/directional_led_entity.h"
#include "argos3/core/utility/datatypes/color.h"
#include <string>
void CTrajectoryLoopFunctions::Init(TConfigurationNode& t_tree) {
CSpace::TMapPerType& tKBMap = GetSpace().GetEntitiesByType("kilobot");
std::string unKilobotID;
for(CSpace::TMapPerType::iterator it = tKBMap.begin(); it != tKBMap.end(); ++it) {
/* Create a pointer to the current kilobot */
CKilobotEntity* pcKB = any_cast<CKilobotEntity*>(it->second);
unKilobotID = pcKB->GetId();
std::cout << unKilobotID << " - " << pcKB->GetEmbodiedEntity().GetOriginAnchor().Position << "\n";
}
}
void CTrajectoryLoopFunctions::Reset(){}
void CTrajectoryLoopFunctions::PostStep() {
/* Get the map of all kilobots from the space */
CSpace::TMapPerType& tKBMap = GetSpace().GetEntitiesByType("kilobot");
/* Go through them */
std::string unKilobotID;
argos::CColor colorKilobotLed;
for(CSpace::TMapPerType::iterator it = tKBMap.begin(); it != tKBMap.end(); ++it) {
/* Create a pointer to the current kilobot */
CKilobotEntity* pcKB = any_cast<CKilobotEntity*>(it->second);
CDirectionalLEDEntity* pcKBLED = any_cast<CDirectionalLEDEntity*>(it->second);
unKilobotID = pcKB->GetId();
colorKilobotLed = pcKBLED->GetColor();
std::cout << unKilobotID << " - " << colorKilobotLed << " - " << pcKB->GetEmbodiedEntity().GetOriginAnchor().Position << "\n";
}
}
REGISTER_LOOP_FUNCTIONS(CTrajectoryLoopFunctions, "trajectory_loop_functions")
