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