order_daemon.h

Go to the documentation of this file.

/*
 * Copyright 2022 Technical University of Munich, Chair of Materials Handling,
 * Material Flow, Logistics – All Rights Reserved
 * 
 * You may use, distribute and modify this code under the terms of the 3-clause
 * BSD license. You should have received a copy of that license with this file.
 * If not, please write to {kontakt.fml@ed.tum.de}.
 */
 
#ifndef ORDER_DAEMON_H
#define ORDER_DAEMON_H
#include <ros/ros.h>
#include "daemon.h"
#include "std_msgs/String.h"
#include "std_msgs/Bool.h"
#include "vda5050_msgs/Order.h"
#include "vda5050_msgs/ActionState.h"
#include "vda5050_msgs/AGVPosition.h"
#include "vda5050_msgs/Edge.h"
#include "vda5050_msgs/Node.h"
#include <string>
 
using namespace std;
 
class CurrentOrder
{
        private:
        string orderId;
        int orderUpdateId;
        string zoneSetId;
        public:
        bool actionsFinished;
        bool actionCancellationComplete;
        deque<vda5050_msgs::Edge> edgeStates;
        deque<vda5050_msgs::Node> nodeStates;
        vector<string> actionStates;
        CurrentOrder(const vda5050_msgs::Order::ConstPtr& incomingOrder);
 
        bool compareOrderId(string orderIdToCompare);
 
        string compareOrderUpdateId(int orderUpdateIdToCompare);
 
        bool compareBase(string startOfNewBaseNodeId, int startOfNewBaseSequenceId);
 
        string getOrderId();
 
        int getOrderUpdateId();
 
        void setOrderUpdateId(int incomingUpdateId);
 
        bool isActive();
 
        string findNodeEdge(int currSequenceId);
 
        vda5050_msgs::Node getLastNodeInBase();
 
        void sendActions(ros::Publisher actionPublisher);
 
        void sendNodeStates(ros::Publisher nodeStatesPublisher);
 
        void sendEdgeStates(ros::Publisher edgeStatesPublisher);
};
 
class AGVPosition
{
        private:
        float x;
        float y;
        float theta;
        string mapId;
        public:
 
        AGVPosition();
        
        void updatePosition(float new_x, float new_y, float new_theta,
                string new_mapId);
 
        float nodeDistance(float node_x, float node_y);
 
        float getTheta();
};
 
class OrderDaemon: public Daemon
{
        private:
        vector<CurrentOrder> currentOrders;
        AGVPosition agvPosition;
        ros::Subscriber orderCancelSub;
        ros::Subscriber agvPositionSub;
        ros::Subscriber allActionsCancelledSub;
        ros::Publisher orderActionPub;
        ros::Publisher orderCancelPub;
        ros::Publisher orderTriggerPub;
        ros::Publisher nodeStatesPub;
        ros::Publisher edgeStatesPub;
        ros::Publisher lastNodeIdPub;
        ros::Publisher lastNodeSequenceIdPub;
        ros::Publisher orderIdPub;
        ros::Publisher orderUpdateIdPub;
        protected:
        vector<string> ordersToCancel;
        bool isDriving;
        int currSequenceId;
        public:
        OrderDaemon();
 
        void LinkPublishTopics(ros::NodeHandle *nh);
 
        void LinkSubscriptionTopics(ros::NodeHandle *nh);
 
        bool validationCheck(const vda5050_msgs::Order::ConstPtr& msg);
 
        bool inDevRange(vda5050_msgs::Node node);
 
        void triggerNewActions(string nodeOrEdge);
 
        void sendMotionCommand();
 
        void OrderCallback(const vda5050_msgs::Order::ConstPtr& msg);
    
    void OrderCancelRequestCallback(const std_msgs::String::ConstPtr& msg);
 
        void allActionsCancelledCallback(const std_msgs::String::ConstPtr& msg);
 
    void ActionStateCallback(const vda5050_msgs::ActionState::ConstPtr& msg);
 
    void AgvPositionCallback(const vda5050_msgs::AGVPosition::ConstPtr& msg);
 
    void DrivingCallback(const std_msgs::Bool::ConstPtr& msg);
 
        void startNewOrder(const vda5050_msgs::Order::ConstPtr& msg);
 
        void appendNewOrder(const vda5050_msgs::Order::ConstPtr& msg);
        
        void updateExistingOrder(const vda5050_msgs::Order::ConstPtr& msg);
 
        void UpdateOrders();
 
        void orderUpdateError(string orderId, int orderUpdateId);
 
        void orderValidationError(string orderId, int orderUpdateId);
 
};
 
#endif