/*
* ROS callbacks start here...
*/
void ROSVelocityCallback(const geometry_msgs::Twist::ConstPtr& msg)
{
JAUS::VelocityState velocityState;
velocityState.SetVelocityX(msg->linear.x);
velocityState.SetYawRate(msg->angular.z);
velocityState.SetTimeStamp(JAUS::Time::GetUtcTime());
// Save the data to the service
velocityStateSensor->SetVelocityState(velocityState);
// ROS_INFO("Got ROS data: [%f]", msg->linear.x);
}
////////////////////////////////////////////////////////////////////////////////////
///
/// \brief This is a helper method to simulate some physics.
///
////////////////////////////////////////////////////////////////////////////////////
void RobotPrimitiveDriver::UpdatePhysics(const double timeSeconds)
{
// Simulate some simple physics based on
// desired mThrust and mSteering
// to reach local waypoints.
double linearDistance = timeSeconds*mThrust*0.025;
double linearVelocity = linearDistance/timeSeconds;
double rotation = timeSeconds*mSteering*CxUtils::CxToRadians(1);
double rotationRate = rotation/timeSeconds;
double yaw = mGlobalPose.GetYaw();
JAUS::Point3D localPosChange(linearDistance, 0, 0);
localPosChange = localPosChange.Rotate(yaw, JAUS::Point3D::Z);
CxUtils::Wgs worldPositionWgs(mGlobalPose.GetLatitude(),
mGlobalPose.GetLongitude(),
mGlobalPose.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.
mGlobalPose.SetYaw(yaw);
mGlobalPose.SetLatitude(worldPositionWgs.mLatitude);
mGlobalPose.SetLongitude(worldPositionWgs.mLongitude);
mGlobalPose.SetAltitude(worldPositionWgs.mElevation);
mGlobalPose.SetTimeStamp(JAUS::Time(true));
mVelocityState.SetVelocityX(linearVelocity);
mVelocityState.SetYawRate(rotationRate);
mVelocityState.SetTimeStamp(JAUS::Time(true));
}
////////////////////////////////////////////////////////////////////////////////////
///
/// \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;
}
/*
* 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;
}
