JAUS++: jaus++/2.0/src/jaus/mobility/drivers/globalwaypointlistdriver.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/globalwaypointlistdriver.h"
00042 #include "jaus/core/component.h"
00043 #include <iostream>
00044 #include <iomanip>
00045 
00046 using namespace JAUS;
00047 
00048 const std::string GlobalWaypointListDriver::Name = "urn:jaus:jss:mobility:GlobalWaypointListDriver";
00049 
00057 GlobalWaypointListDriver::GlobalWaypointListDriver(const double updateRateHz) : ListDriver(Service::ID(GlobalWaypointListDriver::Name),
00058                                                                                            Service::ID(ListManager::Name))
00059 {
00060 }
00061 
00062 
00068 GlobalWaypointListDriver::~GlobalWaypointListDriver()
00069 {
00070 }
00071 
00072 
00083 void GlobalWaypointListDriver::ExecuteList(const double speedMetersPerSecond)
00084 {
00085     Mutex::ScopedLock lock(&mListDriverMutex);
00086     if(speedMetersPerSecond > 0.0)
00087     {
00088         mSpeedMetersPerSecond = speedMetersPerSecond;
00089     }
00090 }
00091 
00092 
00103 bool GlobalWaypointListDriver::IsElementSupported(const JAUS::Message* message) const
00104 {
00105     const JAUS::SetGlobalWaypoint* command = dynamic_cast<const JAUS::SetGlobalWaypoint*>(message);
00106     return command != NULL ? true : false;
00107 }
00108 
00109 
00121 void GlobalWaypointListDriver::Receive(const Message* message)
00122 {
00123     switch(message->GetMessageCode())
00124     {
00125     case QUERY_GLOBAL_WAYPOINT:
00126         {
00127             Mutex::ScopedLock lock(&mListDriverMutex);
00128             const QueryGlobalWaypoint* query = dynamic_cast<const QueryGlobalWaypoint*>(message);
00129 
00130             if(query == NULL)
00131             {
00132                 return;
00133             }
00134 
00135             ReportGlobalWaypoint report(message->GetSourceID(), GetComponentID());
00136             Element current = GetActiveListElement();
00137             if(current.mpElement)
00138             {
00139                 SetGlobalWaypoint* wp = dynamic_cast<SetGlobalWaypoint*>(current.mpElement);
00140                 if(wp)
00141                 {
00142                     UInt pv1 = query->GetPresenceVector();
00143                     UInt pv2 = wp->GetPresenceVector();
00144 
00145                     report.SetLatitude(wp->GetLatitude());
00146                     report.SetLongitude(wp->GetLongitude());
00147                     if( (pv2 & (pv1 & ReportGlobalWaypoint::PresenceVector::Altitude)) > 0) { report.SetAltitude(wp->GetAltitude()); }
00148                     if( (pv2 & (pv1 & ReportGlobalWaypoint::PresenceVector::Roll)) > 0) { report.SetRoll(wp->GetRoll()); }
00149                     if( (pv2 & (pv1 & ReportGlobalWaypoint::PresenceVector::Pitch)) > 0) { report.SetPitch(wp->GetPitch()); }
00150                     if( (pv2 & (pv1 & ReportGlobalWaypoint::PresenceVector::Yaw)) > 0) { report.SetYaw(wp->GetYaw()); }
00151                     if( (pv2 & (pv1 & ReportGlobalWaypoint::PresenceVector::WaypointTolerance)) > 0) { report.SetWaypointTolerance(wp->GetWaypointTolerance()); }
00152                     if( (pv2 & (pv1 & ReportGlobalWaypoint::PresenceVector::PathTolerance)) > 0) { report.SetPathTolerance(wp->GetPathTolerance()); }    
00153                 }
00154             }
00155             Send(&report);
00156         }
00157         break;
00158     case REPORT_GLOBAL_WAYPOINT:
00159     default:
00160         ListDriver::Receive(message);
00161         break;
00162     }
00163 }
00164 
00165 
00177 Message* GlobalWaypointListDriver::CreateMessage(const UShort messageCode) const
00178 {
00179     switch(messageCode)
00180     {
00181     case SET_GLOBAL_WAYPOINT:
00182         return new SetGlobalWaypoint();
00183         break;
00184     case QUERY_GLOBAL_WAYPOINT:
00185         return new QueryGlobalWaypoint();
00186         break;
00187     case REPORT_GLOBAL_WAYPOINT:
00188         return new ReportGlobalWaypoint();
00189         break;
00190     default:
00191         return ListDriver::CreateMessage(messageCode);
00192         break;
00193     }
00194     return NULL;
00195 }
00196 
00197 
00198 
00214 bool GlobalWaypointListDriver::IsEventSupported(const Events::Type type,
00215                                                 const double requestedPeriodicRate,
00216                                                 const Message* queryMessage,
00217                                                 double& confirmedPeriodicRate,
00218                                                 std::string& errorMessage) const
00219 {
00220     // Support any type of event for QUERY_GLOBAL_WAYPOINT data.
00221     if(queryMessage->GetMessageCode() == QUERY_GLOBAL_WAYPOINT)
00222     {
00223         confirmedPeriodicRate = requestedPeriodicRate;
00224         return true;
00225     }
00226     // Support any type of event for QUERY_TRAVEL_SPEED data.
00227     else if(queryMessage->GetMessageCode() == QUERY_TRAVEL_SPEED)
00228     {
00229         confirmedPeriodicRate = requestedPeriodicRate;
00230         return true;
00231     }
00232     return false;
00233 }
00234 
00235 
00245 bool GlobalWaypointListDriver::GenerateEvent(const Events::Subscription& info) const
00246 {
00247     if(info.mpQueryMessage->GetMessageCode() == QUERY_GLOBAL_WAYPOINT)
00248     {
00249         const QueryGlobalWaypoint* query = dynamic_cast<const QueryGlobalWaypoint*>(info.mpQueryMessage);
00250 
00251         if(query == NULL)
00252         {
00253             return false;
00254         }
00255 
00256         ReportGlobalWaypoint report;
00257         Element current = GetActiveListElement();
00258         if(current.mpElement)
00259         {
00260             SetGlobalWaypoint* wp = dynamic_cast<SetGlobalWaypoint*>(current.mpElement);
00261             if(wp)
00262             {
00263                 UInt pv1 = query->GetPresenceVector();
00264                 UInt pv2 = wp->GetPresenceVector();
00265 
00266                 report.SetLatitude(wp->GetLatitude());
00267                 report.SetLongitude(wp->GetLongitude());
00268                 if( (pv2 & (pv1 & ReportGlobalWaypoint::PresenceVector::Altitude)) > 0) { report.SetAltitude(wp->GetAltitude()); }
00269                 if( (pv2 & (pv1 & ReportGlobalWaypoint::PresenceVector::Roll)) > 0) { report.SetRoll(wp->GetRoll()); }
00270                 if( (pv2 & (pv1 & ReportGlobalWaypoint::PresenceVector::Pitch)) > 0) { report.SetPitch(wp->GetPitch()); }
00271                 if( (pv2 & (pv1 & ReportGlobalWaypoint::PresenceVector::Yaw)) > 0) { report.SetYaw(wp->GetYaw()); }
00272                 if( (pv2 & (pv1 & ReportGlobalWaypoint::PresenceVector::WaypointTolerance)) > 0) { report.SetWaypointTolerance(wp->GetWaypointTolerance()); }
00273                 if( (pv2 & (pv1 & ReportGlobalWaypoint::PresenceVector::PathTolerance)) > 0) { report.SetPathTolerance(wp->GetPathTolerance()); }    
00274             }
00275         }
00276         SendEvent(info, &report);
00277         return true;
00278     }
00279     else if(info.mpQueryMessage->GetMessageCode() == QUERY_TRAVEL_SPEED)
00280     {
00281         Mutex::ScopedLock lock(&mListDriverMutex);
00282         const QueryTravelSpeed* query = dynamic_cast<const QueryTravelSpeed*>(info.mpQueryMessage);
00283         if(query == NULL)
00284         {
00285             return false;
00286         }
00287         ReportTravelSpeed report;
00288         report.SetSpeed(mSpeedMetersPerSecond);
00289         SendEvent(info, &report);
00290         return true;
00291     }
00292 
00293     return false;
00294 }
00295 
00296 
00302 JAUS::SetGlobalWaypoint GlobalWaypointListDriver::GetCurrentWaypoint() const
00303 {
00304     Element current = GetActiveListElement();
00305     if(current.mpElement)
00306     {
00307         JAUS::SetGlobalWaypoint* cmd = dynamic_cast<JAUS::SetGlobalWaypoint*>(current.mpElement);
00308         if(cmd)
00309         {
00310             return *cmd;
00311         }
00312     }
00313     return JAUS::SetGlobalWaypoint();
00314 }
00315 
00316 
00322 std::vector<JAUS::SetGlobalWaypoint> GlobalWaypointListDriver::GetWaypointList() const
00323 {
00324     // Get local waypoint list.
00325     JAUS::Element::Map elementList = GetElementList();
00326     // Convert to SetGlobalWaypointCommands
00327     JAUS::Element::Map::iterator listElement;
00328     std::vector<JAUS::SetGlobalWaypoint> commandList;
00329     for(listElement = elementList.begin();
00330         listElement != elementList.end();
00331         listElement++)
00332     {
00333         if(listElement->second.mpElement->GetMessageCode() == JAUS::SET_GLOBAL_WAYPOINT)
00334         {
00335             commandList.push_back(*( (JAUS::SetGlobalWaypoint *)(listElement->second.mpElement)) );
00336         }
00337     }
00338     return commandList;
00339 }
00340 
00341 
00347 void GlobalWaypointListDriver::PrintStatus() const
00348 {
00349     std::vector<JAUS::SetGlobalWaypoint> waypoints = GetWaypointList();
00350     if(IsExecuting() == true)
00351     {
00352         std::cout << "[" << GetServiceID().ToString() << "] - Driving to Waypoint [" << GetActiveListElementID() << "]:\n";
00353         std::cout << "There are " << waypoints.size() << " Waypoints in the List.\n";
00354         std::cout << "Execution Speed (m/s): " << std::fixed << std::setprecision(2) << mSpeedMetersPerSecond << std::endl;
00355     }
00356     else
00357     {
00358         std::cout << "[" << GetServiceID().ToString() << "] - Idle\n";
00359         std::cout << "There are " << waypoints.size() << " Waypoints in the List.\n";
00360     }
00361 }
00362 
00363 
00364 /*  End of File */