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