////////////////////////////////////////////////////////////////////////////////////
///
/// \brief This method verifies a minimum number of capabilities
/// were initialized correctly.
///
/// \return True on success, false on failure.
///
////////////////////////////////////////////////////////////////////////////////////
bool GlobalInfo::VerifyInitialization()
{
// Verify that global info has everything.
JAUS::Component* component = GetComponent();
if(component)
{
if(component->GetService(JAUS::GlobalPoseSensor::Name) &&
component->GetService(JAUS::Microcontroller::Name) &&
component->GetService(JAUS::VideoSubscriber::Name) &&
component->GetService(JAUS::RangeSubscriber::Name))
{
// Add check here to verify that we have GPS, Compass, Video
// and any other required data from Baseline.
bool result = true;
result &= IsValueSet(AI::Names::YawDegrees);
return result;
}
}
return false;
}
int main(int argc, char* argv[])
{
JAUS::Component component;
JAUS::Subsystem* judge_subsystem;
std::cout << "Initializing JAUS component... ";
if (initialize_jaus(&component) < 0)
{
std::cout << "Failed!" << std::endl;
return -1;
}
std::cout << "OKkk" << std::endl;
std::cout << "Searching for Judges' COP... ";
if ((judge_subsystem = discover_judge(&component)) == NULL)
{
std::cout << "Failed!" << std::endl;
return -1;
}
std::cout << "Success (Identification: " << judge_subsystem->mIdentification << ")" << std::endl;
std::cout << "READY, We are now listening to ROS topics and updating current JAUS state..." << std::endl;
JAUS::Management* management_service = (JAUS::Management*)component.GetService(JAUS::Management::Name);
JAUS::Time::Stamp display_time = JAUS::Time::GetUtcTimeMs();
while(management_service->GetStatus() != JAUS::Management::Status::Shutdown)
{
if (CxUtils::GetChar() == 'q')
break;
if (JAUS::Time::GetUtcTimeMs() - display_time > 2000)
{
management_service->PrintStatus(); // display a status message
display_time = JAUS::Time::GetUtcTimeMs();
}
}
component.Shutdown();
return 0;
}
////////////////////////////////////////////////////////////////////////////////////
///
/// \brief Generates fake waypoints to simulate control unit for testing.
///
////////////////////////////////////////////////////////////////////////////////////
void RobotSim::UseFakeWaypoints()
{
JAUS::Address controllerID(42, 1, 1);
mLocalWaypointDriverComponent.AccessControlService()->SetControllerID(controllerID);
mLocalWaypointDriverComponent.ManagementService()->SetStatus(JAUS::Management::Status::Ready);
JAUS::SetElement command(mLocalWaypointDriverComponent.GetComponentID(), controllerID);
command.SetRequestID(1);
JAUS::Point3D::List localWaypoints;
// Add more waypoints for testing as needed.
localWaypoints.push_back(JAUS::Point3D(10, 0, 0));
localWaypoints.push_back(JAUS::Point3D(10, 10, 0));
localWaypoints.push_back(JAUS::Point3D(0, 10, 0));
localWaypoints.push_back(JAUS::Point3D(0, 0, 0));
for(unsigned int i = 0; i < (unsigned int)localWaypoints.size(); i++)
{
JAUS::Element e;
// Set the element ID.
e.mID = i + 1;
// If this is the last element (and we are not looping) set the
// next ID to 0, otherwise the value of the next element ID.
if(i < (unsigned int)localWaypoints.size() - 1)
{
e.mNextID = e.mID + 1;
}
// Set previous element.
e.mPrevID = i;
// Populate the element message which is the command to be executed.
JAUS::SetLocalWaypoint* message = new JAUS::SetLocalWaypoint();
message->SetX(localWaypoints[i].mX);
message->SetY(localWaypoints[i].mY);
// Save the pointer
e.mpElement = message;
// Push completed element onto the list.
command.GetElementList()->push_back(e);
}
// Set the command.
mLocalWaypointDriverComponent.TransportService()->Receive(&command);
// Now tell it to start executing.
JAUS::ExecuteList executeCommand(mLocalWaypointDriverComponent.GetComponentID(), controllerID);
executeCommand.SetElementUID(1); // Start at beginning.
executeCommand.SetSpeed(2.0); // Maximum speed.
mLocalWaypointDriverComponent.TransportService()->Receive(&executeCommand);
}
int main(int argc, char* argv[])
{
JAUS::Component component;
// Add services to a component is done using the AddService
// method. Services can only be added before a
// component is initialized using the Initialize method.
// Any attempt to add after will fail. Finally, you
// cannot add multiple of the same service.
// Create a Global Pose Sensor service.
JAUS::GlobalPoseSensor* globalPoseSensor = new JAUS::GlobalPoseSensor();
// Set the update rate of the sensor (Hz). This
// is used to determine what type of periodic
// events the sensor can support.
globalPoseSensor->SetSensorUpdateRate(25);
// Set some global pose values.
JAUS::GlobalPose globalPose;
globalPose.SetLatitude(34.12345);
globalPose.SetLongitude(-116.12345);
globalPose.SetAltitude(100);
globalPose.SetTimeStamp(JAUS::Time(true));
// Set the values.
globalPoseSensor->SetGlobalPose(globalPose);
// Add the sensor service to the component. Remember
// this must be done before initialization. Also,
// the component will delete the service for us
// automatically, so we don't have to!
component.AddService(globalPoseSensor);
// Setup identification info. For questions about this,
// see the previous tutorial(s).
JAUS::Discovery* discoveryService = NULL;
discoveryService = component.DiscoveryService();
discoveryService->SetSubsystemIdentification(JAUS::Subsystem::Vehicle,
"Robot");
discoveryService->SetNodeIdentification("Primary Computer");
discoveryService->SetComponentIdentification("Baseline");
JAUS::Address componentID(1000, 1, 6);
// Initialize!
std::cout << "Initializing component...";
if(component.Initialize(componentID) == false)
{
std::cout << "Failed to initialize component [" << componentID.ToString() << "]\n";
return 0;
}
std::cout << "Success!\n";
// Now go into your main computer loop until the
// component has been told to shutdown.
JAUS::Time::Stamp displayStatusTimeMs = JAUS::Time::GetUtcTimeMs();
while(true)
{
// To "simulate" a real sensor, lets modify
// the latitude like the robot is moving north.
globalPose.SetLatitude(globalPose.GetLatitude() + 0.00001);
globalPose.SetTimeStamp(JAUS::Time(true));
// Update the sensor with the new data. This will
// automatically trigger events if someone (i.e. another
// component) is subscribing to the data from our sensor.
// this is because the Global Pose Sensor class inherits
// from the Events service and has implemented all the
// methods needed to support subscriptions.
globalPoseSensor->SetGlobalPose(globalPose);
JAUS::Management* managementService = NULL;
managementService = component.ManagementService();
if(managementService->GetStatus() == JAUS::Management::Status::Shutdown)
{
// Exit program.
break;
}
if(JAUS::Time::GetUtcTimeMs() - displayStatusTimeMs > 500)
{
// Print out status of the service.
component.GetService(JAUS::GlobalPoseSensor::Name)->PrintStatus(); std::cout << std::endl;
displayStatusTimeMs = JAUS::Time::GetUtcTimeMs();
}
if(CxUtils::GetChar() == 27)
{
break;
}
CxUtils::SleepMs(100);
}
// Shutdown your component completely. Any
// services added or belonging to the component
// will be deleted.
component.Shutdown();
return 0;
}
int main(int argc, char* argv[])
{
JAUS::Component component;
// Setup identification info. For questions about this,
// see the previous tutorial(s).
JAUS::Discovery* discoveryService = NULL;
discoveryService = component.DiscoveryService();
discoveryService->SetSubsystemIdentification(JAUS::Subsystem::Vehicle,
"Robot");
discoveryService->SetNodeIdentification("Primary Computer");
discoveryService->SetComponentIdentification("Baseline");
JAUS::Address componentID(1000, 1, 2);
// Initialize!
std::cout << "Initializing component...";
if(component.Initialize(componentID) == false)
{
std::cout << "Failed to initialize component [" << componentID.ToString() << "]\n";
return 0;
}
std::cout << "Success!\n";
// Now go into your main computer loop until the
// component has been told to shutdown.
JAUS::Time::Stamp displayStatusTimeMs = JAUS::Time::GetUtcTimeMs();
while(true)
{
JAUS::Management* managementService = NULL;
managementService = component.ManagementService();
if(managementService->GetStatus() == JAUS::Management::Status::Shutdown)
{
// Exit program.
break;
}
if(JAUS::Time::GetUtcTimeMs() - displayStatusTimeMs > 500)
{
// Use the discovery service to get a list of
// discovered subsystems. (see below for how to clear map).
JAUS::Subsystem::Map discoveredSubsystems;
discoveryService->GetSubsystems(discoveredSubsystems);
std::cout << "======================================================\n";
JAUS::Subsystem::Map::iterator subsystem;
// The map is indexed by the subsystem number.
for(subsystem = discoveredSubsystems.begin();
subsystem != discoveredSubsystems.end();
subsystem++)
{
std::cout << "Subsystem: "
<< subsystem->first
<< " Identification: "
<< subsystem->second->mIdentification
<< std::endl;
// Lets see if it has specific service we
// want to communicate with. For this tutorial
// let's check for the Liveness service, which
// supports the Query Heartbeat Pulse and
// Report Heartbeat Pulse messages.
// We can use the subsystem data structure to
// get a list of components on the subsystem that
// have the service, so we can send a message to it.
JAUS::Address::List componentsWithLiveness;
componentsWithLiveness = subsystem->second->GetComponentsWithService(JAUS::Liveness::Name);
JAUS::Address::List::iterator c;
for(c = componentsWithLiveness.begin();
c != componentsWithLiveness.end();
c++)
{
// First, make sure it is not the
// component we are using, because we
// want to talk to a different one.
if( (*c) != component.GetComponentID())
{
// Now that we have the ID of
// component with the Liveness
// service, lets send a query message
// and wait for the response.
// Setup the query message to send.
JAUS::QueryHeartbeatPulse query;
query.SetDestinationID( (*c) );
query.SetSourceID(component.GetComponentID());
// This is the response message we want.
JAUS::ReportHeartbeatPulse response;
// Send and see if we got a
// response, but only wait up to 1 second
// for a response. Default value for waiting
// is 100 ms.
std::cout << "\tSending Query to " << c->ToString() << std::endl;
if(component.Send(&query, &response, 1000))
{
std::cout << "\tReceived Response Message!\n\t";
response.Print();
}
}
}
// The above steps can be used to find any component
// with a specific service you wish to communicate with.
}
// Make sure you delete the subsystem map when
// you are done with it, otherwise you will have a
// memory leak.
JAUS::Subsystem::DeleteSubsystemMap(discoveredSubsystems);
displayStatusTimeMs = JAUS::Time::GetUtcTimeMs();
}
if(CxUtils::GetChar() == 27)
{
break;
}
CxUtils::SleepMs(1);
}
// Shutdown your component completely. Any
// services added or belonging to the component
// will be deleted.
component.Shutdown();
return 0;
}
int main(int argc, char* argv[])
{
if (argc < 3)
{
std::cout << "Error: Arguments must include IP and Address" << std::endl;
return 1;
}
char* ip_address = argv[1];
char* comp_address = argv[2];
cout << "ip_address: " << ip_address << " comp_address: " << comp_address << std::endl;
JAUS::Component component;
JAUS::Discovery* discoveryService = nullptr;
JAUS::Transport* transportService = nullptr;
discoveryService = (JAUS::Discovery*)component.GetService(JAUS::Discovery::Name);
discoveryService->SetSubsystemIdentification(JAUS::Subsystem::Vehicle,"Snowbots");
discoveryService->SetNodeIdentification("Main");
discoveryService->SetComponentIdentification("Baseline");
int comp_id = 5000;
JAUS::Address componentID(comp_id,1,1);
discoveryService->EnableDebugMessages(true);
while(component.Initialize(componentID)==false){
std::cout << "Failed to initialize [" << componentID.ToString() << "]" << std::endl;
comp_id++;
componentID(comp_id,1,1);
}
std::cout << "Success!" << std::endl;
transportService = (JAUS::Transport*) component.GetService(JAUS::Transport::Name);
transportService->LoadSettings("services.xml");
const JAUS::Address comp_address_id = component.GetComponentID();
if(!transportService->IsInitialized())
{
transportService->Initialize(comp_address_id);
}
// Create connection to OCP for the JAUS Interoperability Challenge using JUDP.
/*transportService->AddNetworkConnection(JAUS::Address(8000, 1, 1),
std::string(ip_address),
3794);*/
JAUS::Management* managementService = nullptr;
managementService = (JAUS::Management*)component.GetService(JAUS::Management::Name);
JAUS::Time::Stamp displayStatusTimeMs = JAUS::Time::GetUtcTimeMs();
while(true)
{
if(managementService->GetStatus() == JAUS::Management::Status::Shutdown)
{
break;
}
if(JAUS::Time::GetUtcTimeMs() - displayStatusTimeMs > 500)
{
std::cout << "==================" << std::endl;
managementService->PrintStatus();
discoveryService->PrintStatus(); //std::cout << std::endl;
transportService->PrintStatus();
std::cout << std::endl;
displayStatusTimeMs = JAUS::Time::GetUtcTimeMs();
}
CxUtils::SleepMs(1);
}
component.Shutdown();
return 0;
}
////////////////////////////////////////////////////////////////////////////////////
///
/// \brief Entry point of jaus_challenge_2010.cpp
///
/// This program demonstrates everything required to complete the JAUS
/// Interoperability Challenge for 2010. In this program a simulated robot
/// is created which can drive to local waypoints. This program has
/// run against the JAUS Validation Tool (JVT) and the OCP of the
/// JAUS Interopability Challange during the Autonomous Surface Vehicle
/// Competition, passing all requirements.
///
/// All you have to do is integrate on your own robot, providing real
/// sensor values and generating real control of your bot!
///
////////////////////////////////////////////////////////////////////////////////////
int main(int argc, char* argv[])
{
JAUS::Component component;
//component.AddService(new JAUS::JTCPClient());
// Setting timeout for control to 0 (disables timeout of control).
// This is done because the JVT and OCP do not query for the timeout period
// and may not send messages to re-acquire/maintain control within the
// normal 2 second timeout window.
component.AccessControlService()->SetTimeoutPeriod(0);
// Add the services we want/need for our component. By
// default, the component object already has the Core Service set
// (e.g. Discovery, Events, Access Control, Management).
// The Discovery service automatically handles finding other JAUS
// components, so you do not need to generate any Query Messages
// (e.g. Query Identification) yourself, because Discovery does it
// for you.
// In this test program, we are making a simulated robot, which
// requires the following mobility services.
JAUS::GlobalPoseSensor* globalPoseSensor = new JAUS::GlobalPoseSensor();
globalPoseSensor->SetSensorUpdateRate(25); // Updates at 25 Hz (used for periodic events).
component.AddService(globalPoseSensor);
JAUS::LocalPoseSensor* localPoseSensor = new JAUS::LocalPoseSensor();
localPoseSensor->SetSensorUpdateRate(25); // Updates at 25 Hz (used for periodic events).
component.AddService(localPoseSensor);
JAUS::VelocityStateSensor* velocityStateSensor = new JAUS::VelocityStateSensor();
velocityStateSensor->SetSensorUpdateRate(25); // Updates at 25 Hz (used for periodic events).
component.AddService(velocityStateSensor);
// Need a List Manager Service for Local Waypoint List Driver. You can
// ignore this service for the Underwater Vehicle Competition.
component.AddService(new JAUS::ListManager());
JAUS::LocalWaypointListDriver* localWaypointListDriver = new JAUS::LocalWaypointListDriver();
component.AddService(localWaypointListDriver);
// Set Vehicle Identification Information. Available options for the
// parameters are JAUS::Subsystem::OCU and JAUS::Subsystem::Vehicle. Since
// we are not an OCU, we use Vehicle. Finally, the string represents the type
// of robot you have (e.g. Segway RMP, XUV). This is different than the
// name of your vehicle, but you can use that if you want. Your subsystem number
// is your unique identifier.
component.DiscoveryService()->SetSubsystemIdentification(JAUS::Subsystem::Vehicle,
"Simulation");
// Initialize component with a given ID. Remember, you must
// add all your services before you initialize the component, because you
// cannot add them after initialization. All services will be deleted by
// the component on program exit.
if(component.Initialize(JAUS::Address(gSubsystemID, gNodeID, gComponentID)) == false)
{
std::cout << "Failed to Initialize Component.\n";
return 0;
}
// Set state to Standby since we have initialized OK.
component.ManagementService()->SetStatus(JAUS::Management::Status::Standby);
// Now that we are initialized, lets create a fixed connection to the
// JVT for testing. Since the JVT and the OCP of the JAUS
// Interoperability Challenge do not support multicast for Discovery, you
// must make a direct connection to them.
JAUS::JTCPClient* transportService = NULL;
transportService = (JAUS::JTCPClient*)component.TransportService();
/*
// Create connection to JAUS Validation Tool (JVT)
transportService->AddConnection("24.42.140.203", JAUS::Address(90, 1, 1));
// Create connection to OCP for the JAUS Interoperability Challenge.
transportService->AddConnection("192.168.1.42", JAUS::Address(42, 1, 1));
*/
// Set an initial global pose.
JAUS::GlobalPose globalPose;
globalPose.SetLatitude(32.703356);
globalPose.SetLongitude(-117.253919);
globalPose.SetAltitude(300);
globalPose.SetPositionRMS(0.0);
globalPose.SetRoll(0.0);
globalPose.SetPitch(0.0);
globalPose.SetYaw(CxUtils::CxToRadians(45));
globalPose.SetAttitudeRMS(0.0);
globalPose.SetTimeStamp(JAUS::Time::GetUtcTime());
// Save the data to the service.
globalPoseSensor->SetGlobalPose(globalPose);
// You can set local pose data using global pose
// as it is automatically converted to local pose data for you.
// How convenient!
localPoseSensor->SetLocalPose(globalPose);
// Set an initial velocity state.
JAUS::VelocityState velocityState;
velocityState.SetVelocityX(0.0);
velocityState.SetYawRate(0.0);
velocityState.SetVelocityRMS(0.0);
velocityState.SetTimeStamp(JAUS::Time::GetUtcTime());
// Save the data to the service.
velocityStateSensor->SetVelocityState(velocityState);
JAUS::Time::Stamp printTimeMs = 0;
double timeDiff = 0.33; // Used for simulation of robot physics.
while(CxUtils::GetChar() != 27 &&
component.ManagementService()->GetStatus() != JAUS::Management::Status::Shutdown)
{
double thrust = 25;
double steering = 0;
// Simulate some simple physics based on
// desired thrust and steering
// to reach local waypoints.
double linearDistance = timeDiff*thrust*0.025;
double linearVelocity = linearDistance/timeDiff;
double rotation = timeDiff*steering*CxUtils::CxToRadians(1);
double rotationRate = rotation/timeDiff;
double yaw = globalPose.GetYaw();
JAUS::Point3D localPosChange(linearDistance, 0, 0);
localPosChange = localPosChange.Rotate(yaw, JAUS::Point3D::Z);
CxUtils::Wgs worldPositionWgs(globalPose.GetLatitude(),
globalPose.GetLongitude(),
globalPose.GetAltitude());
CxUtils::Utm worldPositionUtm(worldPositionWgs);
worldPositionUtm.mNorthing += localPosChange.mX;
worldPositionUtm.mEasting += localPosChange.mY;
yaw = CxUtils::Orientation::AddToAngle(yaw, rotation);
// Convert to WGS
worldPositionWgs << worldPositionUtm;
// Save newly calculated position and orientation.
globalPose.SetYaw(yaw);
globalPose.SetLatitude(worldPositionWgs.mLatitude);
globalPose.SetLongitude(worldPositionWgs.mLongitude);
globalPose.SetAltitude(worldPositionWgs.mElevation);
globalPose.SetTimeStamp(JAUS::Time(true));
velocityState.SetVelocityX(linearVelocity);
velocityState.SetYawRate(rotationRate);
velocityState.SetTimeStamp(JAUS::Time(true));
// Save global pose information.
globalPoseSensor->SetGlobalPose(globalPose);
// The Local Pose Sensor service supports converting
// global pose data to local pose data. It automatically
// calculates offsets and relative changes for you.
localPoseSensor->SetLocalPose(globalPose);
// Save velocity state information.
velocityStateSensor->SetVelocityState(velocityState);
if(JAUS::Time::GetUtcTimeMs() - printTimeMs > 500)
{
// Print status of services.
std::cout << "\n======================================================\n";
component.AccessControlService()->PrintStatus(); std::cout << std::endl;
component.ManagementService()->PrintStatus(); std::cout << std::endl;
globalPoseSensor->PrintStatus(); std::cout << std::endl;
localPoseSensor->PrintStatus(); std::cout << std::endl;
velocityStateSensor->PrintStatus(); std::cout << std::endl;
localWaypointListDriver->PrintStatus();
printTimeMs = JAUS::Time::GetUtcTimeMs();
}
// Exit if escape key pressed.
if(CxUtils::GetChar() == 27)
{
break;
}
CxUtils::SleepMs((unsigned int)(timeDiff*1000.0));
}
// Don't delete services, they are
// deleted by the Component class.
// Shutdown any components associated with our subsystem.
component.Shutdown();
return 0;
}
int main(int argc, char* argv[]) {
ros::init(argc, argv, "jaus");
ros::NodeHandle self;
JAUS::Component component;
JAUS::Discovery* discoveryService = nullptr;
JAUS::Transport* transportService = nullptr;
localPoseSensor = new JAUS::LocalPoseSensor();
velocityStateSensor = new JAUS::VelocityStateSensor();
LocalWaypointDriver* localWaypointDriver = new LocalWaypointDriver(self.advertise<sb_msgs::MoveCommand>(MOVE_COMMAND_TOPIC,100),localPoseSensor,nullptr);
LocalWaypointListDriver* localWaypointListDriver = new LocalWaypointListDriver(localWaypointDriver);
localWaypointDriver->setListDriver(localWaypointListDriver);
{
JAUS::ReportLocalPose localPose;
localPose.SetX(1.0);
localPose.SetY(1.0);
localPose.SetYaw(0.1);
localPoseSensor->SetLocalPose(localPose);
}
{
JAUS::ReportVelocityState state;
state.SetVelocityX(2);
state.SetYawRate(0.1);
velocityStateSensor->SetVelocityState(state);
}
self.subscribe("robot_state",100,onRobotStateChange);
self.subscribe("GPS_COORD",100,onNewWaypoint);
component.AddService(localPoseSensor);
component.AddService(localWaypointDriver);
component.AddService(velocityStateSensor);
discoveryService = (JAUS::Discovery*)component.GetService(JAUS::Discovery::Name);
discoveryService->SetSubsystemIdentification(JAUS::Subsystem::Vehicle,"Snowbots");
discoveryService->SetNodeIdentification("Main");
discoveryService->SetComponentIdentification("Baseline");
int comp_id = 5000;
JAUS::Address componentID(comp_id,1,1);
discoveryService->EnableDebugMessages(true);
while(component.Initialize(componentID)==false) {
std::cout << "Failed to initialize [" << componentID.ToString() << "]" << std::endl;
comp_id++;
componentID(comp_id,1,1);
}
std::cout << "Success!" << std::endl;
transportService = (JAUS::Transport*) component.GetService(JAUS::Transport::Name);
transportService->LoadSettings("services.xml");
const JAUS::Address comp_address_id = component.GetComponentID();
if(!transportService->IsInitialized()) {
transportService->Initialize(comp_address_id);
}
JAUS::Management* managementService = nullptr;
managementService = (JAUS::Management*)component.GetService(JAUS::Management::Name);
JAUS::Time::Stamp displayStatusTimeMs = JAUS::Time::GetUtcTimeMs();
ros::Rate loop_rate(10);
while(ros::ok()) {
if(managementService->GetStatus() == JAUS::Management::Status::Shutdown) {
break;
}
if(JAUS::Time::GetUtcTimeMs() - displayStatusTimeMs > 500) {
std::cout << "==================" << std::endl;
managementService->PrintStatus();
discoveryService->PrintStatus();
transportService->PrintStatus();
std::cout << std::endl;
displayStatusTimeMs = JAUS::Time::GetUtcTimeMs();
}
ros::spinOnce();
loop_rate.sleep();
}
component.Shutdown();
return 0;
}
/*
* Main program entry point.
*/
int main(int argc, char* argv[])
{
std::cout << "Initializing ROS component... ";
// ROS initialization stuff
ros::init(argc, argv, "jaus");
ros::NodeHandle n;
// subscriptions
subscriberCmdVel = n.subscribe(ROSTopicCmdVelocity, 1000, ROSVelocityCallback);
subscriberOdom = n.subscribe(ROSTopicOdometry, 1000, ROSOdometryCallback);
// publications
publisherSetLocalPose = n.advertise<nav_msgs::Odometry>(ROSTopicSetLocalPose, 1000);
// -- we just use std_msgs::String for waypoints for now, until we can all agree on
// some custom ROS messages for setting local pose, and setting waypoints...
publisherSetWaypoints = n.advertise<std_msgs::String>(ROSTopicSetWaypoints, 1000);
std::cout << "OK" << std::endl;
// JAUS initialization stuff
JAUS::Component component;
JAUS::Subsystem* judgeSubsystem;
std::cout << "Initializing JAUS component... ";
if (InitializeJAUSComponent(&component) < 0)
{
std::cout << "Failed!" << std::endl;
return -1;
}
std::cout << "OK" << std::endl;
std::cout << "Searching for Judges' COP... ";
if ((judgeSubsystem = DiscoverJudgeSubsystem(&component)) == NULL)
{
std::cout << "Failed!" << std::endl;
return -1;
}
std::cout << "Success (Identification: " << judgeSubsystem->mIdentification << ")" << std::endl;
/*
* Set initial states for services
*/
// Set an initial local pose
JAUS::LocalPose localPose;
localPose.SetX(0.0);
localPose.SetY(0.0);
localPose.SetTimeStamp(JAUS::Time::GetUtcTime());
localPoseSensor->SetLocalPose(localPose);
// Set an initial velocity state
JAUS::VelocityState velocityState;
velocityState.SetVelocityX(0.0);
velocityState.SetYawRate(0.0);
velocityState.SetTimeStamp(JAUS::Time::GetUtcTime());
velocityStateSensor->SetVelocityState(velocityState);
// now loop forever
std::cout << "READY, We are now listening to ROS topics and updating current JAUS state..." << std::endl;
JAUS::Management* managementService = (JAUS::Management*)component.GetService(JAUS::Management::Name);
JAUS::Time::Stamp displayStatusTimeMs = JAUS::Time::GetUtcTimeMs();
while (true) // (managementService->GetStatus() != JAUS::Management::Status::Shutdown)
{
if (CxUtils::GetChar() == 'q')
break;
if (JAUS::Time::GetUtcTimeMs() - displayStatusTimeMs > 2000)
{
managementService->PrintStatus(); // display a status message
displayStatusTimeMs = JAUS::Time::GetUtcTimeMs();
}
ros::spinOnce(); // process ROS callbacks
CxUtils::SleepMs(10); // save some CPU time
}
component.Shutdown(); // this will take care of cleanup
return 0;
}
////////////////////////////////////////////////////////////////////////////////////
///
/// \brief Simple simulation of a component with a Local Pose Sensor using
/// JTCP.
///
/// \param[in] subsystemID Subsystem ID number.
/// \param[in] nodeID Node ID number.
///
////////////////////////////////////////////////////////////////////////////////////
int RunServer(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,
"Local Pose JTCP Tutorial");
// Change the transport service to use JTCP
component.AddService(new JAUS::JTCPClient());
// Add any other services. In this case local pose.
JAUS::LocalPoseSensor* localPoseSensor = new JAUS::LocalPoseSensor();
component.AddService(localPoseSensor);
// Initialize component and communication.
if(component.Initialize(id) == false)
{
std::cout << "Failed to initialize JAUS component with ID: " << id.ToString() << std::endl;
return 0;
}
// Configure local pose information.
JAUS::LocalPose localPose;
localPose.SetX(0);
localPose.SetY(0);
localPose.SetZ(0);
localPose.SetYaw(0);
localPose.SetTimeStamp(JAUS::Time(true));
JAUS::Time printTime(true);
while(CxUtils::GetChar() != 27 && component.ManagementService()->GetStatus() != JAUS::Management::Status::Shutdown)
{
// Fake a position change.
localPose.SetY(localPose.GetY() + .01);
localPose.SetTimeStamp(JAUS::Time(true));
// Save the data. Anyone who is subscribing will be
// notified of the change automatically.
localPoseSensor->SetLocalPose(localPose);
// Only print to screen every now and then.
if(JAUS::Time(true) - printTime > .5)
{
std::cout << "=======================================================\n";
localPoseSensor->PrintStatus();
component.EventsService()->PrintStatus();
printTime.SetCurrentTime();
}
// Don't overload the CPU
CxUtils::SleepMs(1);
}
// Do shutdown
component.Shutdown();
return 0;
}
////////////////////////////////////////////////////////////////////////////////////
///
/// \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[])
{
// Create a component. By default a component
// has all the services of the Core Service set:
// - Transport (JUDP)
// - Control
// - Discovery
// - Events
// - Liveness
// - Time
// - Management
JAUS::Component component;
// The Transport service is used to send
// and receive messages to other JAUS components. All
// other services use the Transport service. The
// default transport type for JAUS++ is UDP communication
// using the JUDP class.
// The Discovery service is used to find
// other JAUS components and services on the
// network using the Transport service. In JAUS++
// Discovery will automatically find these components,
// make connections to them, and keep track of what
// services they have.
// The first thing we must do for a component is
// configure its identification. This is done by
// using the Discovery Service. Get a pointer
// to the service:
JAUS::Discovery* discoveryService = NULL;
discoveryService = (JAUS::Discovery*)component.GetService(JAUS::Discovery::Name);
// Alternative method:
// discoveryService = component.DiscoveryService();
// Set the type of subsystem the component is for. Subsystem
// types available are currently Vehicle, or OCU. The string
// name "Robot" represents the type or category of platform.
// You must set the subsystem identification before you will be
// able to initialize your component.
discoveryService->SetSubsystemIdentification(JAUS::Subsystem::Vehicle,
"Robot");
// You can also set identification information for the component
// and node that it is on.
discoveryService->SetNodeIdentification("Primary Computer");
discoveryService->SetComponentIdentification("Baseline");
// Now that we have setup our identification information we
// can initialize our component. First, create the
// component ID.
JAUS::Address componentID(1000, 1, 1);
// Initialize!
std::cout << "Initializing component...";
if(component.Initialize(componentID) == false)
{
std::cout << "Failed to initialize component [" << componentID.ToString() << "]\n";
return 0;
}
std::cout << "Success!\n";
// Now go into your main computer loop until the
// component has been told to shutdown.
JAUS::Time::Stamp displayStatusTimeMs = JAUS::Time::GetUtcTimeMs();
while(true)
{
// Let's check the "state" of our component. This
// is done using the Management service.
// A component can be in the following states:
// - Initialized
// - Ready
// - Standby
// - Shutdown
// - Failure
// - Emergency
JAUS::Management* managementService = NULL;
managementService = (JAUS::Management*)component.GetService(JAUS::Management::Name);
// Alternative method:
//managementService = component.ManagementService();
if(managementService->GetStatus() == JAUS::Management::Status::Shutdown)
{
// Exit program.
break;
}
if(JAUS::Time::GetUtcTimeMs() - displayStatusTimeMs > 500)
{
std::cout << "======================================================\n";
// Print status of the service.
managementService->PrintStatus(); std::cout << std::endl;
displayStatusTimeMs = JAUS::Time::GetUtcTimeMs();
}
if(CxUtils::GetChar() == 27)
{
break;
}
CxUtils::SleepMs(1);
}
// Shutdown your component completely. Any
// services added or belonging to the component
// will be deleted.
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;
}
int main(int argc, char **argv)
{
SICK::Sick laser;
MessageCallback cb;
JAUS::Component component;
// Add the services we want our component to have
// beyond the core set.
JAUS::RangeSensor* rangeSensor = new JAUS::RangeSensor();
// Setup the configuration of a range sensing device.
JAUS::RangeSensorConfig rangeSensorConfig;
rangeSensorConfig.mID = 0;
rangeSensorConfig.mName = "LIDAR";
rangeSensorConfig.mMinRange = 0.0;
rangeSensorConfig.mMaxRange = 9.0; // 9 meter range.
rangeSensorConfig.mUnitType = JAUS::RangeSensorConfig::MM; // Range values are in mm.
rangeSensorConfig.mScanAngle = CxUtils::CxToRadians(180); // [-90, 90] degrees.
rangeSensorConfig.mAngleIncrement = CxUtils::CxToRadians(0.5); // 0.5 degree increment.
rangeSensor->AddRangeDevice(rangeSensorConfig);
// Add to component.
component.AddService(rangeSensor);
cb.mpRangeSensor = rangeSensor;
// Register a callback to process the received
// messages.
laser.RegisterCallback(&cb);
cout << "Connecting to SICK...";
laser.InitializeWithDefaults("/dev/ttyUSB0", 38400);
cout << "Success!\n";
// In this example, we are simulating a component on part of a simulated
// robot.
component.DiscoveryService()->SetSubsystemIdentification(JAUS::Subsystem::Vehicle,
"Simulation");
// Initialize component component with component given ID.
if(component.InitializeWithUniqueID(2000) == false)
{
std::cout << "Failed to Initialize JAUS Component.\n";
return 0;
}
std::cout << "JAUS Component [" << component.GetComponentID().ToString() << "] Initialized!\n";
while ( true )
{
if (CxUtils::GetChar() == 27)
{
break;
}
CxUtils::SleepMs(1);
}
// Make sure to shutdown the laser.
laser.Shutdown();
component.Shutdown();
return 0;
}
