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