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); }
/* * 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; }