JAUS++: jaus++/2.0/src/jaus/mobility/drivers/primitivedriver.cpp Source File

Go to the documentation of this file.
00001 
00002 
00003 
00004 
00005 
00006 
00007 
00008 
00009 
00010 
00011 
00012 
00013 
00014 
00015 
00016 
00017 
00018 
00019 
00020 
00021 
00022 
00023 
00024 
00025 
00026 
00027 
00028 
00029 
00030 
00031 
00032 
00033 
00034 
00035 
00036 
00037 
00038 
00039 
00040 
00041 #include "jaus/mobility/drivers/primitivedriver.h"
00042 #include "jaus/core/component.h"
00043 
00044 using namespace JAUS;
00045 
00046 const std::string PrimitiveDriver::Name = "urn:jaus:jss:mobility:PrimitiveDriver";
00047 
00048 
00054 PrimitiveDriver::PrimitiveDriver() : Management::Child(Service::ID(PrimitiveDriver::Name), 
00055                                                        Service::ID(Management::Name))
00056 {
00057 }
00058 
00059 
00065 PrimitiveDriver::~PrimitiveDriver()
00066 {
00067 }
00068 
00069 
00077 SetWrenchEffort PrimitiveDriver::GetCurrentWrenchEffort() const
00078 {
00079     Mutex::ScopedLock lock(&mPrimitiveDriverMutex);
00080     return mCurrentWrenchEffort;
00081 }
00082 
00083 
00089 Time PrimitiveDriver::GetWrenchEffortTime() const
00090 {
00091     Mutex::ScopedLock lock(&mPrimitiveDriverMutex);
00092     return mWrenchEffortTime;
00093 }
00094 
00095 
00105 bool PrimitiveDriver::GenerateEvent(const Events::Subscription& info) const
00106 {
00107     if(info.mpQueryMessage->GetMessageCode() == QUERY_WRENCH_EFFORT)
00108     {
00109         Mutex::ScopedLock lock(&mPrimitiveDriverMutex);
00110         const QueryWrenchEffort* query = dynamic_cast<const QueryWrenchEffort*>(info.mpQueryMessage);
00111 
00112         if(query == NULL)
00113         {
00114             return false;
00115         }
00116 
00117         ReportWrenchEffort report;
00118         CreateReportFromQuery(query, report);
00119         SendEvent(info, &report);
00120 
00121         return true;
00122     }
00123 
00124     return false;
00125 }
00126 
00127 
00143 bool PrimitiveDriver::IsEventSupported(const Events::Type type,
00144                                        const double requestedPeriodicRate,
00145                                        const Message* queryMessage,
00146                                        double& confirmedPeriodicRate,
00147                                        std::string& errorMessage) const
00148 {
00149     // Support any type of event for QUERY_WRENCH_EFFORT data.
00150     if(queryMessage->GetMessageCode() == QUERY_WRENCH_EFFORT)
00151     {
00152         confirmedPeriodicRate = requestedPeriodicRate;
00153         return true;
00154     }
00155     return false;
00156 }
00157 
00158 
00170 void PrimitiveDriver::Receive(const Message *message)
00171 {
00172     switch(message->GetMessageCode())
00173     {
00174     case QUERY_WRENCH_EFFORT:
00175         {
00176             const JAUS::QueryWrenchEffort* query = dynamic_cast<const JAUS::QueryWrenchEffort*>(message);
00177             if(query)
00178             {
00179                 Mutex::ScopedLock lock(&mPrimitiveDriverMutex);
00180                 ReportWrenchEffort report;
00181                 CreateReportFromQuery(query, report);
00182                 Send(&report);
00183             }
00184         }
00185         break;
00186     case SET_WRENCH_EFFORT:
00187         {
00188             const JAUS::SetWrenchEffort* command = dynamic_cast<const JAUS::SetWrenchEffort*>(message);
00189             if(command)
00190             {
00191                 if(GetComponent()->ManagementService()->GetStatus() == Management::Status::Ready)
00192                 {
00193                     Mutex::ScopedLock lock(&mPrimitiveDriverMutex);
00194                     if(SetWrenchEffort(command))
00195                     {
00196                         mCurrentWrenchEffort = *command;
00197                         mWrenchEffortTime.SetCurrentTime();
00198                         SignalEvent(REPORT_WRENCH_EFFORT);
00199                     }
00200                 }
00201             }
00202         }
00203         break;
00204     default:
00205         break;
00206     }
00207 }
00208 
00209 
00221 Message* PrimitiveDriver::CreateMessage(const UShort messageCode) const
00222 {
00223     Message* message = NULL;
00224     switch(messageCode)
00225     {
00226     case QUERY_WRENCH_EFFORT:
00227         message = new JAUS::QueryWrenchEffort();
00228         break;
00229     case REPORT_WRENCH_EFFORT:
00230         message = new JAUS::ReportWrenchEffort();
00231         break;
00232     case SET_WRENCH_EFFORT:
00233         message = new JAUS::SetWrenchEffort();
00234         break;
00235     default:
00236         message = NULL;
00237         break;
00238     };
00239     return message;
00240 }
00241 
00242 
00248 void PrimitiveDriver::PrintStatus() const
00249 {
00250     std::cout << "[" << GetServiceID().ToString() << "] - Current Wrench Effort:\n";
00251     if(GetStatus() != Management::Status::Ready)
00252     {
00253         std::cout << "Standby - No Valid Wrench Effort.\n";
00254     }
00255     else
00256     {
00257         JAUS::SetWrenchEffort wrench;
00258         mPrimitiveDriverMutex.Lock();
00259         wrench = mCurrentWrenchEffort;
00260         mPrimitiveDriverMutex.Unlock();
00261         wrench.PrintMessageBody();
00262     }
00263 }
00264 
00265 
00274 void PrimitiveDriver::CreateReportFromQuery(const QueryWrenchEffort* query,
00275                                             ReportWrenchEffort& report) const
00276 {
00277     Mutex::ScopedLock lock(&mPrimitiveDriverMutex);
00278     report.ClearMessage();
00279     report.SetDestinationID(query->GetSourceID());
00280     report.SetSourceID(GetComponentID());
00281     UInt pv1 = query->GetPresenceVector();
00282     UInt pv2 = mCurrentWrenchEffort.GetPresenceVector();
00283 
00284     if( (pv2 & (pv1 & WrenchEffort::PresenceVector::PropulsiveLinearEffortX)) > 0) { report.SetPropulsiveLinearEffortX(mCurrentWrenchEffort.GetPropulsiveLinearEffortX()); }
00285     if( (pv2 & (pv1 & WrenchEffort::PresenceVector::PropulsiveLinearEffortY)) > 0) { report.SetPropulsiveLinearEffortY(mCurrentWrenchEffort.GetPropulsiveLinearEffortY()); }
00286     if( (pv2 & (pv1 & WrenchEffort::PresenceVector::PropulsiveLinearEffortZ)) > 0) { report.SetPropulsiveLinearEffortZ(mCurrentWrenchEffort.GetPropulsiveLinearEffortZ()); }
00287     if( (pv2 & (pv1 & WrenchEffort::PresenceVector::PropulsiveRotationalEffortX)) > 0) { report.SetPropulsiveRotationalEffortX(mCurrentWrenchEffort.GetPropulsiveRotationalEffortX()); }
00288     if( (pv2 & (pv1 & WrenchEffort::PresenceVector::PropulsiveRotationalEffortY)) > 0) { report.SetPropulsiveRotationalEffortY(mCurrentWrenchEffort.GetPropulsiveRotationalEffortY()); }
00289     if( (pv2 & (pv1 & WrenchEffort::PresenceVector::PropulsiveRotationalEffortZ)) > 0) { report.SetPropulsiveRotationalEffortZ(mCurrentWrenchEffort.GetPropulsiveRotationalEffortZ()); }
00290     if( (pv2 & (pv1 & WrenchEffort::PresenceVector::ResistiveLinearEffortX)) > 0) { report.SetResistiveLinearEffortX(mCurrentWrenchEffort.GetResistiveLinearEffortX()); }
00291     if( (pv2 & (pv1 & WrenchEffort::PresenceVector::ResistiveLinearEffortY)) > 0) { report.SetResistiveLinearEffortY(mCurrentWrenchEffort.GetResistiveLinearEffortY()); }
00292     if( (pv2 & (pv1 & WrenchEffort::PresenceVector::ResistiveLinearEffortZ)) > 0) { report.SetResistiveLinearEffortZ(mCurrentWrenchEffort.GetResistiveLinearEffortZ()); }
00293     if( (pv2 & (pv1 & WrenchEffort::PresenceVector::ResistiveRotationalEffortX)) > 0) { report.SetResistiveRotationalEffortX(mCurrentWrenchEffort.GetResistiveRotationalEffortX()); }
00294     if( (pv2 & (pv1 & WrenchEffort::PresenceVector::ResistiveRotationalEffortY)) > 0) { report.SetResistiveRotationalEffortY(mCurrentWrenchEffort.GetResistiveRotationalEffortY()); }
00295     if( (pv2 & (pv1 & WrenchEffort::PresenceVector::ResistiveRotationalEffortZ)) > 0) { report.SetResistiveRotationalEffortZ(mCurrentWrenchEffort.GetResistiveRotationalEffortZ()); }
00296 }
00297 
00298 
00299 /*  End of File */