void
MainLoopAspectIniFin::init(Thread *thread)
{
MainLoopAspect *mainloop_thread;
mainloop_thread = dynamic_cast<MainLoopAspect *>(thread);
if (mainloop_thread == NULL) {
throw CannotInitializeThreadException("Thread '%s' claims to have the "
"MainLoopAspect, but RTTI says it "
"has not. ",
thread->name());
}
if (thread->opmode() != Thread::OPMODE_WAITFORWAKEUP) {
throw CannotInitializeThreadException("MainLoopAspect thread must operate "
"in wait-for-wakeup mode.");
}
try {
mainloop_uc_.add(mainloop_thread);
mainloop_thread->init_MainLoopAspect(btexec_);
thread->add_notification_listener(this);
} catch (Exception &e) {
CannotInitializeThreadException ce("Main loop thread failed to initialize");
ce.append(e);
throw ce;
}
}
/** Initialize
* @param thread thread
*/
void
RobotMemoryIniFin::init(Thread *thread)
{
RobotMemoryAspect *robot_memory_thread;
robot_memory_thread = dynamic_cast<RobotMemoryAspect *>(thread);
if (robot_memory_thread == NULL) {
throw CannotInitializeThreadException("Thread '%s' claims to have the "
"RobotMemoryAspect, but RTTI says it "
"has not. ", thread->name());
}
if (! robot_memory_) {
throw CannotInitializeThreadException("robot_memory object has not been set.");
}
robot_memory_thread->init_RobotMemoryAspect(robot_memory_);
}
void
ROSAspectIniFin::init(Thread *thread)
{
ROSAspect *ros_thread;
ros_thread = dynamic_cast<ROSAspect *>(thread);
if (ros_thread == NULL) {
throw CannotInitializeThreadException("Thread '%s' claims to have the "
"ROSAspect, but RTTI says it "
"has not. ", thread->name());
}
if (! __rosnode) {
throw CannotInitializeThreadException("ROS node handle has not been set.");
}
ros_thread->init_ROSAspect(__rosnode);
}
/** Finilize
* @param thread thread
*/
void
RobotMemoryIniFin::finalize(Thread *thread)
{
RobotMemoryAspect *robot_memory_thread;
robot_memory_thread = dynamic_cast<RobotMemoryAspect *>(thread);
if (robot_memory_thread == NULL) {
throw CannotInitializeThreadException("Thread '%s' claims to have the "
"RobotMemoryAspect, but RTTI says it "
"has not. ", thread->name());
}
robot_memory_thread->finalize_RobotMemoryAspect();
}
void
BlockedTimingAspectIniFin::init(Thread *thread)
{
BlockedTimingAspect *blocked_timing_thread;
blocked_timing_thread = dynamic_cast<BlockedTimingAspect *>(thread);
if (blocked_timing_thread == 0) {
throw CannotInitializeThreadException("Thread '%s' claims to have the "
"BlockedTimingAspect, but RTTI says it "
"has not. ",
thread->name());
}
if (thread->opmode() != Thread::OPMODE_WAITFORWAKEUP) {
throw CannotInitializeThreadException("Thread '%s' not in WAITFORWAKEUP mode"
" (required for BlockedTimingAspect)",
thread->name());
}
blocked_timing_thread->init_BlockedTimingAspect(thread);
}
void
NavGraphAspectIniFin::init(Thread *thread)
{
NavGraphAspect *navgraph_thread;
navgraph_thread = dynamic_cast<NavGraphAspect *>(thread);
if (navgraph_thread == NULL) {
throw CannotInitializeThreadException("Thread '%s' claims to have the "
"NavGraphAspect, but RTTI says it "
"has not. ", thread->name());
}
navgraph_thread->navgraph = navgraph_;
}
void
PointCloudAspectIniFin::init(Thread *thread)
{
PointCloudAspect *pcl_thread;
pcl_thread = dynamic_cast<PointCloudAspect *>(thread);
if (pcl_thread == NULL) {
throw CannotInitializeThreadException("Thread '%s' claims to have the "
"PointCloudAspect, but RTTI says it "
"has not. ", thread->name());
}
pcl_thread->init_PointCloudAspect(__pcl_manager);
}
void
LoggingAspectIniFin::init(Thread *thread)
{
LoggingAspect *logging_thread;
logging_thread = dynamic_cast<LoggingAspect *>(thread);
if (logging_thread == 0) {
throw CannotInitializeThreadException("Thread '%s' claims to have the "
"LoggingAspect, but RTTI says it "
"has not. ", thread->name());
}
logging_thread->init_LoggingAspect(__logger);
}
void
ThreadProducerAspectIniFin::init(Thread *thread)
{
ThreadProducerAspect *thread_producer_thread;
thread_producer_thread = dynamic_cast<ThreadProducerAspect *>(thread);
if (thread_producer_thread == 0) {
throw CannotInitializeThreadException("Thread '%s' claims to have the "
"ThreadProducerAspect, but RTTI says it "
"has not. ", thread->name());
}
thread_producer_thread->init_ThreadProducerAspect(__collector);
}
void
TransformAspectIniFin::init(Thread *thread)
{
TransformAspect *transform_thread;
transform_thread = dynamic_cast<TransformAspect *>(thread);
if (transform_thread == NULL) {
throw CannotInitializeThreadException("Thread '%s' claims to have the "
"TransformAspect, but RTTI says it "
"has not. ", thread->name());
}
transform_thread->init_TransformAspect(__blackboard, __transformer, thread->name());
}
void
ConfigurableAspectIniFin::init(Thread *thread)
{
ConfigurableAspect *configurable_thread;
configurable_thread = dynamic_cast<ConfigurableAspect *>(thread);
if (configurable_thread == NULL) {
throw CannotInitializeThreadException("Thread '%s' claims to have the "
"ConfigurableAspect, but RTTI says it "
"has not. ", thread->name());
}
configurable_thread->init_ConfigurableAspect(__config);
}
void
WebviewAspectIniFin::init(Thread *thread)
{
WebviewAspect *webview_thread;
webview_thread = dynamic_cast<WebviewAspect *>(thread);
if (webview_thread == NULL) {
throw CannotInitializeThreadException("Thread '%s' claims to have the "
"WebviewAspect, but RTTI says it "
"has not. ", thread->name());
}
webview_thread->init_WebviewAspect(__url_manager, __nav_manager,
__request_manager, __rest_api_manager);
}
void
SyncPointAspectIniFin::finalize(Thread *thread)
{
SyncPointAspect *syncpoint_thread;
syncpoint_thread = dynamic_cast<SyncPointAspect *>(thread);
if (syncpoint_thread == NULL) {
throw CannotInitializeThreadException("Thread '%s' claims to have the "
"SyncPointManagerAspect, but RTTI says it "
"has not. ", thread->name());
}
syncpoint_thread->finalize_SyncPointAspect(thread, __syncpoint_manager);
}
void
LoggerAspectIniFin::init(Thread *thread)
{
LoggerAspect *logger_thread;
logger_thread = dynamic_cast<LoggerAspect *>(thread);
if (logger_thread == 0) {
throw CannotInitializeThreadException("Thread '%s' claims to have the "
"LoggerAspect, but RTTI says it "
"has not. ", thread->name());
}
try {
__employer->add_logger(logger_thread->get_logger());
} catch (Exception &e) {
CannotInitializeThreadException ce("Thread has LoggerAspect but Logger "
"could not be added.");
ce.append(e);
throw ce;
} catch (...) {
throw CannotInitializeThreadException("Thread has LoggerAspect but Logger "
"could not be added.");
}
}
void
BlockedTimingAspectIniFin::finalize(Thread *thread)
{
BlockedTimingAspect *blocked_timing_thread;
blocked_timing_thread = dynamic_cast<BlockedTimingAspect *>(thread);
if (blocked_timing_thread == 0) {
throw CannotInitializeThreadException("Thread '%s' claims to have the "
"BlockedTimingAspect, but RTTI says it "
"has not. ",
thread->name());
}
blocked_timing_thread->finalize_BlockedTimingAspect(thread);
}
void
AspectManager::init(Thread *thread)
{
Aspect *aspected_thread = dynamic_cast<Aspect *>(thread);
if (aspected_thread != NULL) { // thread has aspects to initialize
const std::list<const char *> &aspects = aspected_thread->get_aspects();
std::list<const char *> initialized;
try {
std::list<const char *>::const_iterator i;
for (i = aspects.begin(); i != aspects.end(); ++i) {
if (inifins_.find(*i) == inifins_.end()) {
throw CannotInitializeThreadException("Thread '%s' has the %s, "
"but no initializer is known.",
thread->name(),
*i);
}
inifins_[*i]->init(thread);
initialized.push_back(*i);
}
for (i = aspects.begin(); i != aspects.end(); ++i) {
threads_[*i].push_back(thread);
}
} catch (CannotInitializeThreadException &e) {
std::list<const char *>::const_reverse_iterator i;
for (i = initialized.rbegin(); i != initialized.rend(); ++i) {
inifins_[*i]->finalize(thread);
}
throw;
} catch (Exception &e) {
std::list<const char *>::const_reverse_iterator i;
for (i = initialized.rbegin(); i != initialized.rend(); ++i) {
inifins_[*i]->finalize(thread);
}
CannotInitializeThreadException ce;
ce.append(e);
throw ce;
}
}
}
void
MainLoopAspectIniFin::finalize(Thread *thread)
{
MainLoopAspect *mainloop_thread;
mainloop_thread = dynamic_cast<MainLoopAspect *>(thread);
if (mainloop_thread == NULL) {
throw CannotInitializeThreadException("Thread '%s' claims to have the "
"MainLoopAspect, but RTTI says it "
"has not. ", thread->name());
}
try {
__employer->set_mainloop_thread(NULL);
__mainloop_uc.remove(mainloop_thread);
} catch (Exception &e) {
CannotFinalizeThreadException ce("Failed to remove time source");
ce.append(e);
throw;
}
}
void
BlackBoardAspectIniFin::init(Thread *thread)
{
BlackBoardAspect *blackboard_thread;
blackboard_thread = dynamic_cast<BlackBoardAspect *>(thread);
if (blackboard_thread == NULL) {
throw CannotInitializeThreadException("Thread '%s' claims to have the "
"BlackBoardAspect, but RTTI says it "
"has not. ", thread->name());
}
BlackBoard *bb;
if (blackboard_thread->blackboard_owner_name_) {
bb = new BlackBoardWithOwnership(__blackboard,
blackboard_thread->blackboard_owner_name_);
} else {
bb = new BlackBoardWithOwnership(__blackboard, thread->name());
}
blackboard_thread->init_BlackBoardAspect(bb);
}
