//////////////////////////////////////////////////////////////////////////////////// /// /// \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); }
void JAUS_Controller::WaypointCallback::ProcessMessage(const JAUS::Message* message) { MST_JAUS::JAUS_out msg; if(message->GetMessageCode() == JAUS::SET_LOCAL_POSE) { ROS_INFO("SetLocalPose JAUS message received."); msg.set_local_pose = true; const JAUS::SetLocalPose* setLocalPose = dynamic_cast<const JAUS::SetLocalPose*>(message); msg.waypoint_pose_x.push_back(setLocalPose->GetX()); msg.waypoint_pose_y.push_back(setLocalPose->GetY()); msg.pose_yaw.push_back(setLocalPose->GetYaw()); } else if(message->GetMessageCode() == JAUS::SET_ELEMENT) { ROS_INFO("SetElement JAUS message received."); msg.set_waypoints = true; const JAUS::SetElement* setElement = dynamic_cast<const JAUS::SetElement*>(message); for(unsigned int i = 0; i < setElement->GetElementList()->size(); ++i) { const JAUS::Element* element = &setElement->GetElementList()->at(i); JAUS::SetLocalWaypoint waypoint; waypoint.Read(element->mPayload); msg.waypoint_id.push_back(element->mID); msg.waypoint_previous_id.push_back(element->mPrevID); msg.waypoint_next_id.push_back(element->mNextID); msg.waypoint_pose_x.push_back(waypoint.GetX()); msg.waypoint_pose_y.push_back(waypoint.GetY()); std::cout<<"id: "<<element->mID<<' '<<waypoint.GetX()<<' '<<waypoint.GetY()<<std::endl; } } else if(message->GetMessageCode() == JAUS::EXECUTE_LIST) { ROS_INFO("ExecuteList JAUS message received."); msg.execute_waypoints = true; const JAUS::ExecuteList* executeList = dynamic_cast<const JAUS::ExecuteList*>(message); msg.speed = executeList->GetSpeed(); } parent->p_Control.publish(msg); }
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()); } } }