コード例 #1
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);
}
コード例 #2
0
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);
}
コード例 #3
0
    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());
            }
        }
    }