/** Loop for generating drive commands. */
void GlobalCommand::CommandThread()
{
JAUS::AccessControl* control = NULL;
JAUS::Management* management = NULL;
control = GetComponent()->AccessControlService();
management = GetComponent()->ManagementService();
while(mCommandThread.QuitThreadFlag() == false && control)
{
JAUS::Address baselineComponentID;
JAUS::Address driverToControl;
JAUS::Message* driveCommandToSend = NULL;
if(baselineComponentID.IsValid() == false)
{
//JAUS::Address::List idList = GetComponent()->DiscoveryService()->GetSubsystem(GetComponentID().mSubsystem)->GetComponentsWithService(JAUS::PrimitiveDriver::Name);
JAUS::Address::List idList = GetComponent()->DiscoveryService()->GetSubsystem(GetComponentID().mSubsystem)->GetComponentsWithService(JAUS::Microcontroller::Name);
if(idList.size() > 0)
{
baselineComponentID = idList.front();
}
}
if(baselineComponentID.IsValid() == false)
{
CxUtils::SleepMs(1);
//std::cout << "Baseline Invalid\n";
continue;
}
// Take control of the correct component based on the
// type of control mode we are. If Power, control Primitive Driver
// component, and release control of Velocity State Driver. Do the
// reverse for Velocity State Driver
JAUS::SetVelocityVector setVelocityVector;
JAUS::SetMicrocontrollerState setMicrocontrollerState;
mCommandMutex.Lock();
if(mControlReady==true)
{
//Control the three orientation angles
setVelocityVector.SetRoll(mDesiredRoll);
setVelocityVector.SetPitch(mDesiredPitch);
setVelocityVector.SetYaw(mDesiredYaw);
//Set the depth in meters, if surface called, force surface
if(mSurfaceFlag)
{
setVelocityVector.SetPositionZ(0);
}
else
{
setVelocityVector.SetPositionZ(mDesiredDepth);
}
//Set the velocities
setVelocityVector.SetVelocityX(mDesiredAxialVel);
setVelocityVector.SetVelocityY(mDesiredLateralVel);
}
mCommandMutex.Unlock();
setVelocityVector.SetSourceID(mGlobalInfo->GetComponentID());
setVelocityVector.SetDestinationID(baselineComponentID);
//setVelocityVector.Print();
mCommandMutex.Lock();
(*setMicrocontrollerState.GetDigitalStates())["Torpedo1"] = FireUpdate(mTorpedo1,mTorpedo1Ticks);
(*setMicrocontrollerState.GetDigitalStates())["Torpedo2"] = FireUpdate(mTorpedo2,mTorpedo2Ticks);
(*setMicrocontrollerState.GetDigitalStates())["Dropper1"] = FireUpdate(mDropper1,mDropper1Ticks);
(*setMicrocontrollerState.GetDigitalStates())["Dropper2"] = FireUpdate(mDropper2,mDropper2Ticks);
(*setMicrocontrollerState.GetDigitalStates())["GrabberArm1"] = FireUpdate(mGrabber1,mGrabber1Ticks);
(*setMicrocontrollerState.GetDigitalStates())["GrabberArm2"] = FireUpdate(mGrabber2,mGrabber2Ticks);
//setMicrocontrollerState
mCommandMutex.Unlock();
mCommandMutex.Lock();
bool estop;
mGlobalInfo->GetInfo(GlobalInfo::EStop, estop);
if (!estop)
{
(*setMicrocontrollerState.GetAnalogStates())["Disp_Upper_Left_Red"] = 255;
(*setMicrocontrollerState.GetAnalogStates())["Disp_Upper_Left_Green"] = 0;
(*setMicrocontrollerState.GetAnalogStates())["Disp_Upper_Left_Blue"] = 0;
(*setMicrocontrollerState.GetAnalogStates())["Disp_Forward_Arrow_Red"] = 255;
(*setMicrocontrollerState.GetAnalogStates())["Disp_Forward_Arrow_Green"] = 0;
(*setMicrocontrollerState.GetAnalogStates())["Disp_Forward_Arrow_Blue"] = 0;
(*setMicrocontrollerState.GetAnalogStates())["Disp_Upper_Right_Red"] = 255;
(*setMicrocontrollerState.GetAnalogStates())["Disp_Upper_Right_Green"] = 0;
(*setMicrocontrollerState.GetAnalogStates())["Disp_Upper_Right_Blue"] = 0;
(*setMicrocontrollerState.GetAnalogStates())["Disp_Left_Arrow_Red"] = 255;
(*setMicrocontrollerState.GetAnalogStates())["Disp_Left_Arrow_Green"] = 0;
(*setMicrocontrollerState.GetAnalogStates())["Disp_Left_Arrow_Blue"] = 0;
(*setMicrocontrollerState.GetAnalogStates())["Disp_Center_Red"] = 255;
(*setMicrocontrollerState.GetAnalogStates())["Disp_Center_Green"] = 0;
(*setMicrocontrollerState.GetAnalogStates())["Disp_Center_Blue"] = 0;
(*setMicrocontrollerState.GetAnalogStates())["Disp_Right_Arrow_Red"] = 255;
(*setMicrocontrollerState.GetAnalogStates())["Disp_Right_Arrow_Green"] = 0;
(*setMicrocontrollerState.GetAnalogStates())["Disp_Right_Arrow_Blue"] = 0;
(*setMicrocontrollerState.GetAnalogStates())["Disp_Lower_Left_Red"] = 255;
(*setMicrocontrollerState.GetAnalogStates())["Disp_Lower_Left_Green"] = 0;
(*setMicrocontrollerState.GetAnalogStates())["Disp_Lower_Left_Blue"] = 0;
(*setMicrocontrollerState.GetAnalogStates())["Disp_Back_Arrow_Red"] = 255;
(*setMicrocontrollerState.GetAnalogStates())["Disp_Back_Arrow_Green"] = 0;
(*setMicrocontrollerState.GetAnalogStates())["Disp_Back_Arrow_Blue"] = 0;
(*setMicrocontrollerState.GetAnalogStates())["Disp_Lower_Right_Red"] = 255;
(*setMicrocontrollerState.GetAnalogStates())["Disp_Lower_Right_Green"] = 0;
(*setMicrocontrollerState.GetAnalogStates())["Disp_Lower_Right_Blue"] = 0;
}
else
{
(*setMicrocontrollerState.GetAnalogStates())["Disp_Upper_Left_Red"] = mUpperLeftRed;
(*setMicrocontrollerState.GetAnalogStates())["Disp_Upper_Left_Green"] = mUpperLeftGreen;
(*setMicrocontrollerState.GetAnalogStates())["Disp_Upper_Left_Blue"] = mUpperLeftBlue;
(*setMicrocontrollerState.GetAnalogStates())["Disp_Forward_Arrow_Red"] = mForwardArrowRed;
(*setMicrocontrollerState.GetAnalogStates())["Disp_Forward_Arrow_Green"] = mForwardArrowGreen;
(*setMicrocontrollerState.GetAnalogStates())["Disp_Forward_Arrow_Blue"] = mForwardArrowBlue;
(*setMicrocontrollerState.GetAnalogStates())["Disp_Upper_Right_Red"] = mUpperRightRed;
(*setMicrocontrollerState.GetAnalogStates())["Disp_Upper_Right_Green"] = mUpperRightGreen;
(*setMicrocontrollerState.GetAnalogStates())["Disp_Upper_Right_Blue"] = mUpperRightBlue;
(*setMicrocontrollerState.GetAnalogStates())["Disp_Left_Arrow_Red"] = mLeftArrowRed;
(*setMicrocontrollerState.GetAnalogStates())["Disp_Left_Arrow_Green"] = mLeftArrowGreen;
(*setMicrocontrollerState.GetAnalogStates())["Disp_Left_Arrow_Blue"] = mLeftArrowBlue;
(*setMicrocontrollerState.GetAnalogStates())["Disp_Center_Red"] = mCenterRed;
(*setMicrocontrollerState.GetAnalogStates())["Disp_Center_Green"] = mCenterGreen;
(*setMicrocontrollerState.GetAnalogStates())["Disp_Center_Blue"] = mCenterBlue;
(*setMicrocontrollerState.GetAnalogStates())["Disp_Right_Arrow_Red"] = mRightArrowRed;
(*setMicrocontrollerState.GetAnalogStates())["Disp_Right_Arrow_Green"] = mRightArrowGreen;
(*setMicrocontrollerState.GetAnalogStates())["Disp_Right_Arrow_Blue"] = mRightArrowBlue;
(*setMicrocontrollerState.GetAnalogStates())["Disp_Lower_Left_Red"] = mLowerLeftRed;
(*setMicrocontrollerState.GetAnalogStates())["Disp_Lower_Left_Green"] = mLowerLeftGreen;
(*setMicrocontrollerState.GetAnalogStates())["Disp_Lower_Left_Blue"] = mLowerLeftBlue;
(*setMicrocontrollerState.GetAnalogStates())["Disp_Back_Arrow_Red"] = mBackArrowRed;
(*setMicrocontrollerState.GetAnalogStates())["Disp_Back_Arrow_Green"] = mBackArrowGreen;
(*setMicrocontrollerState.GetAnalogStates())["Disp_Back_Arrow_Blue"] = mBackArrowBlue;
(*setMicrocontrollerState.GetAnalogStates())["Disp_Lower_Right_Red"] = mLowerRightRed;
(*setMicrocontrollerState.GetAnalogStates())["Disp_Lower_Right_Green"] = mLowerRightGreen;
(*setMicrocontrollerState.GetAnalogStates())["Disp_Lower_Right_Blue"] = mLowerRightBlue;
}
mCommandMutex.Unlock();
setMicrocontrollerState.SetSourceID(mGlobalInfo->GetComponentID());
setMicrocontrollerState.SetDestinationID(baselineComponentID);
bool mHaveControl;
//release in surface
double currDepth;
mGlobalInfo->GetInfo(GlobalInfo::DepthSensor, currDepth);
if(mSurfaceFlag && currDepth<mSurfaceDepth)
{
AccessControlService()->ReleaseComponentControl(baselineComponentID);
}
else if(!AccessControlService()->HaveControl(baselineComponentID))
{
AccessControlService()->RequestComponentControl(baselineComponentID);
}
mHaveControl &= SendCommand(&setMicrocontrollerState, true);
//mHaveControl &= SendCommand(&setWrenchEffort, true);
if(mControlReady==true)
{
mHaveControl &= SendCommand(&setVelocityVector, true);
}
//std::cout << "Here: " << std::endl;
//commandLoop++;
CxUtils::SleepMs(mDelayTimeMs);
/*if(mCommandMode == AI::GlobalCommand::Power)
{
driverToControl = mPrimitiveDriverComponentID;
if(control->HaveControl(mVelocityStateDriverComponentID))
{
control->ReleaseComponentControl(mVelocityStateDriverComponentID);
}
if(!control->HaveControl(driverToControl))
{
this->RequestComponentControl(driverToControl, true, true);
}
driveCommandToSend = &mWrenchEffort;
}
else if(mCommandMode == AI::GlobalCommand::Velocity)
{
driverToControl = mVelocityStateDriverComponentID;
if(control->HaveControl(mPrimitiveDriverComponentID))
{
control->ReleaseComponentControl(mPrimitiveDriverComponentID);
}
if(!control->HaveControl(driverToControl))
{
this->RequestComponentControl(driverToControl, true, true);
}
if(mStandardsModeFlag)
{
driveCommandToSend = &mVelocityCommand;
}
else
{
driveCommandToSend = &mVelocityVector;
}
}*/
/*******driverToControl = mVelocityStateDriverComponentID;
if(control->HaveControl(mPrimitiveDriverComponentID))
{
control->ReleaseComponentControl(mPrimitiveDriverComponentID);
}
if(!control->HaveControl(driverToControl))
{
this->RequestComponentControl(driverToControl, true, true);
}
driveCommandToSend = &mVelocityVector;******/
/*if(mStandardsModeFlag)
{
driveCommandToSend = &mVelocityCommand;
}
if(control->HaveControl(mVelocityStateDriverComponentID))
{
driveCommandToSend = &mVelocityVector;
}*/
// Send drive command.
/******if(driveCommandToSend)
{
CxUtils::Mutex::ScopedLock lock(&mCommandMutex);
driveCommandToSend->SetSourceID(GetComponentID());
driveCommandToSend->SetDestinationID(driverToControl);
Send(driveCommandToSend);
}
CxUtils::SleepMs(mDelayTimeMs);*****/
}
}
////////////////////////////////////////////////////////////////////////////////////
///
/// \brief Simple simulation of a component that subscribes to any Local
/// Pose Sensor data on network using JTCP.
///
/// \param[in] subsystemID Subsystem ID number.
/// \param[in] nodeID Node ID number.
///
////////////////////////////////////////////////////////////////////////////////////
int RunClient(JAUS::UShort subsystemID, JAUS::Byte nodeID)
{
JAUS::Component component;
JAUS::Address id(subsystemID, nodeID, 1);
if(id.IsValid() == false)
{
std::cout << "Invalid JAUS ID: " << id.ToString() << std::endl;
}
// Setup identification data.
component.DiscoveryService()->SetSubsystemIdentification(JAUS::Subsystem::Vehicle,
"Subscriber JTCP Tutorial");
// Change the transport service to use JTCP
component.AddService(new JAUS::JTCPClient());
//((JAUS::JTCPClient*)component.TransportService())->AddConnection("10.171.190.61", JAUS::Address(subsystemID, 1, 1));
//((JAUS::JTCPClient*)component.TransportService())->AddConnection("127.0.0.1", JAUS::Address(subsystemID, 1, 1));
// Add support for reading Local Pose messages without having to
// add a custom service.
component.TransportService()->AddMessageTemplate(new JAUS::ReportLocalPose());
component.TransportService()->AddMessageTemplate(new JAUS::QueryLocalPose());
// Add a callback to get messages when they arrive.
LocalPoseCallback localPoseCallback;
component.TransportService()->RegisterCallback(JAUS::REPORT_LOCAL_POSE, &localPoseCallback);
// Initialize component and communication.
if(component.Initialize(id) == false)
{
std::cout << "Failed to initialize JAUS component with ID: " << id.ToString() << std::endl;
return 0;
}
JAUS::Time printTime(true);
while(CxUtils::GetChar() != 27 && component.ManagementService()->GetStatus() != JAUS::Management::Status::Shutdown)
{
// Look for any subsystems.
JAUS::Subsystem::Map subsystems;
JAUS::Subsystem::Map::iterator subsystem;
component.DiscoveryService()->GetSubsystems(subsystems);
for(subsystem = subsystems.begin();
subsystem != subsystems.end();
subsystem++)
{
// Look for local pose sensors to subscribe to.
JAUS::Address::List sensors = subsystem->second->GetComponentsWithService(JAUS::LocalPoseSensor::Name);
if(sensors.size() > 0)
{
JAUS::Address::List::iterator id;
for(id = sensors.begin();
id != sensors.end();
id++)
{
if(*id != component.GetComponentID() &&
component.EventsService()->HaveSubscription(JAUS::REPORT_LOCAL_POSE, *id) == false)
{
std::cout << "Found new Local Pose Sensors on " << id->ToString() << std::endl;
JAUS::QueryLocalPose queryLocalPose;
// Request all fields.
queryLocalPose.SetPresenceVector(queryLocalPose.GetPresenceVectorMask());
component.EventsService()->RequestPeriodicEvent(*id, &queryLocalPose, 5);
}
}
}
}
JAUS::Subsystem::DeleteSubsystemMap(subsystems);
// Only print to screen every now and then.
if(JAUS::Time(true) - printTime > .5)
{
//std::cout << "=======================================================\n";
//component.EventsService()->PrintStatus();
//printTime.SetCurrentTime();
}
// Don't overload the CPU
CxUtils::SleepMs(1);
}
// Do shutdown
component.Shutdown();
return 0;
}
int main(int argc, char* argv[])
{
JAUS::Component component;
// Testing prioritized message data
component.TransportService()->AddPriorityMessage(JAUS::REPORT_GLOBAL_POSE);
// Add the services our component needs. This has to
// be done before component initialization.
// Second, you cannot add the same service twice.
JAUS::GlobalPoseSensor* globalPoseSensor = new JAUS::GlobalPoseSensor();
JAUS::LocalPoseSensor* localPoseSensor = new JAUS::LocalPoseSensor();
JAUS::VelocityStateSensor* velocityStateSensor = new JAUS::VelocityStateSensor();
JAUS::AccelerationStateSensor* accelerationStateSensor = new JAUS::AccelerationStateSensor();
FakeMicrocontroller* mcu = new FakeMicrocontroller();
// Add to our component.
component.AddService(globalPoseSensor);
component.AddService(localPoseSensor);
component.AddService(velocityStateSensor);
component.AddService(accelerationStateSensor);
component.AddService(mcu);
// Tell our sensor services what component we want them
// to sync there data with (i.e. who to subscribe to.
JAUS::Address syncID(gSubsystemID, gSyncNodeID, gSyncComponentID);
globalPoseSensor->SynchronizeToComponent(syncID);
localPoseSensor->SynchronizeToComponent(syncID);
velocityStateSensor->SynchronizeToComponent(syncID);
accelerationStateSensor->SynchronizeToComponent(syncID);
mcu->SynchronizeToComponent(syncID);
// Example of how to use events callback
EventCallback callback;
component.EventsService()->RegisterCallback(&callback);
// Try load settings files.
// These files determine your UDP network
// settings, what Services to turn on/off
// or any Service specific settings. See the
// example file for settings file format.
if(component.LoadSettings("settings/services.xml") == false)
{
// Working directory probably not set (or not running from output directory), so
// use default values.
component.DiscoveryService()->SetSubsystemIdentification(JAUS::Subsystem::Vehicle,
"Simulation");
// Set optional identification info for this component.
component.DiscoveryService()->SetComponentIdentification("Synchronize");
}
// Initialize component with component given ID.
if(component.Initialize(JAUS::Address(gSubsystemID, gNodeID, gComponentID)) == false)
{
std::cout << "Failed to Initialize Component.\n";
return 0;
}
bool foundSyncID = false;
CxUtils::Time::Stamp printTimeMs = 0;
while(CxUtils::GetChar() != 27)
{
// This is a short example code to dynamically lookup a
// component ID for the service you want to synchronize on your
// subsystem (like in your AI program if the services may run on different computers).
#if 0
// Enable synchronization for each sensor.
if(globalPoseSensor->IsSynchronizing() == false)
{
// Find sensor to sync to.
JAUS::Address::List idList = component.DiscoveryService()->GetSubsystem(gSubsystemID)->GetComponentsWithService(JAUS::GlobalPoseSensor::Name);
if(idList.size() > 0)
{
globalPoseSensor->SynchronizeToComponent(idList.front());
}
}
#endif
if(CxUtils::Time::GetUtcTimeMs() - printTimeMs > 500)
{
// Print status of services.
std::cout << "\n======================================================\n";
globalPoseSensor->PrintStatus(); std::cout << std::endl;
localPoseSensor->PrintStatus(); std::cout << std::endl;
velocityStateSensor->PrintStatus(); std::cout << std::endl;
accelerationStateSensor->PrintStatus(); std::cout << std::endl;
mcu->PrintStatus(); std::cout << std::endl;
printTimeMs = CxUtils::Time::GetUtcTimeMs();
}
CxUtils::SleepMs(1);
}
// Don't delete services, they are
// deleted by the Component class.
// Shutdown any components associated with our subsystem.
component.Shutdown();
return 0;
}