void ROSOdometryCallback(const nav_msgs::Odometry::ConstPtr& msg)
{
JAUS::LocalPose localPose;
localPose.SetX(msg->pose.pose.position.x);
localPose.SetY(msg->pose.pose.position.y);
localPose.SetTimeStamp(JAUS::Time::GetUtcTime());
localPoseSensor->SetLocalPose(localPose);
}
void UpdateWaypointDriver()
{
double thrust = 0;
double steering = 0;
// Are we in an execute state (e.g. received execute command and are
// in ready state for driving). This part is not needed if you are
// doing the Underwater Vehicle Competition, since you are not required to
// drive to local waypoints.
if(mpLocalWaypointListDriver->IsExecuting())
{
// If the current element in the list is 0, then we have reached the
// end of the list. The element ID in the list is set by the OCP using
// the Execute message which the LocalWaypointListDriver service processes
// for you.
if(mpLocalWaypointListDriver->GetActiveListElementID() != 0)
{
// Get the current waypoint in the list.
JAUS::SetLocalWaypoint wp = mpLocalWaypointListDriver->GetCurrentWaypoint();
JAUS::LocalPose pose = mpLocalPoseSensor->GetLocalPose();
// See if we reached the point.
CxUtils::Point3D wpPos(wp.GetX(), wp.GetY(), wp.GetZ());
CxUtils::Point3D myPos(pose.GetX(), pose.GetY(), pose.GetZ());
double distanceToWaypoint = myPos.Distance(wpPos);
double tolerance = 0.25;
// See if the waypoint has a desired tolerance, if not use the
// default value of 0.25 meter radius.
if(wp.IsFieldPresent(JAUS::SetLocalWaypoint::PresenceVector::WaypointTolerance))
{
tolerance = wp.GetWaypointTolerance();
}
if(distanceToWaypoint <= tolerance)
{
// Reached point, advance list.
mpLocalWaypointListDriver->AdvanceListElement();
}
else
{
// Drive to local waypoint by generating a desired
// thrust and steering. For this demo/example program
// an open-loop control system is used.
// Get angle to waypoint.
double angle = atan2(wpPos.mY - myPos.mY, wpPos.mX - myPos.mX);
double angleDiff = CxUtils::Orientation::AngleDiff(pose.GetYaw(), angle);
// Steer to angle
if(fabs(angleDiff) > CxUtils::CxToRadians(5))
{
steering = 30;
if(angleDiff < 0)
{
steering *= -1;
}
}
else
{
// Within 5 degrees of desired heading
// set thrust to 30 to drive, and
// stop rotating.
thrust = 30;
steering = 0;
}
}
}
// Check for control of Primitive Driver component
if(mLocalWaypointDriverComponent.AccessControlService()->HaveControl(mBaselineComponent.GetComponentID()) == false)
{
mLocalWaypointDriverComponent.AccessControlService()->RequestComponentControl(mBaselineComponent.GetComponentID());
// Put component in ready state.
mLocalWaypointDriverComponent.ManagementService()->Resume(mBaselineComponent.GetComponentID());
}
if(mLocalWaypointDriverComponent.AccessControlService()->HaveControl(mBaselineComponent.GetComponentID()))
{
// Send drive command to make robot move!
JAUS::SetWrenchEffort wrenchCommand(mBaselineComponent.GetComponentID(), mLocalWaypointDriverComponent.GetComponentID());
wrenchCommand.SetPropulsiveLinearEffortX(thrust);
wrenchCommand.SetPropulsiveRotationalEffortZ(steering);
mLocalWaypointDriverComponent.Send(&wrenchCommand);
}
}
else
{
// Make sure we release control if we don't need it.
if(mLocalWaypointDriverComponent.AccessControlService()->HaveControl(mBaselineComponent.GetComponentID()))
{
mLocalWaypointDriverComponent.AccessControlService()->ReleaseComponentControl(mBaselineComponent.GetComponentID());
}
}
}
/*
* 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;
}