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