<reponame>nhattruongpham/ROS_experiment<gh_stars>0
#!/usr/bin/env python
import sys
import rospy
import datetime
import threading

from std_msgs.msg import Bool, Int16, String
from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped

from MoveAction import MoveAction 
from WayPoints_tma import WayPoints   

from actionlib_msgs.msg import *		#--- May be deleted if it is not needed ################################

#-----------------------------------------------------------------------------------------------------------------------
class delivery:
    def __init__(self, maxTimeToWait):
        #--- Declare an object of WayPoints class [Delivery plan] ------------------------------------------------------
        self.deliveryPlan = WayPoints()
        self.maxTimeToWait = maxTimeToWait
        #---------------------------------------------------------------------------------------------------------------

        #--- Doc du lieu trang thai tu [move_base/status] topic [status_array = actionlib_msgs.msg.GoalStatusArray()] --
        rospy.Subscriber('move_base/status', GoalStatusArray, self.callback_status)
        #--- Nhan du lieu do android app gui xuong Robot ---------------------------------------------------------------
        rospy.Subscriber('new_goal', String, self.callback_new_goal)
        #--- Nhan du lieu do android app gui xuong Robot ---------------------------------------------------------------
        rospy.Subscriber('cancel_goal', String, self.callback_calcel_goal)

        #--- Neutral node publish data to topic for interface with some app -------------------------------------------- 
        #self.confirmGoal = rospy.Publisher('received_goal', std_msgs.msg.String, queue_size=20)

        self.rate = rospy.Rate(10) #--- 10hz :: 0.1 second  ------------------------------------------------------------
        #--- Create the publisher to send the confirmation to Android App when it was received data from it ------------
        self.confirmGoal = rospy.Publisher('received_goal', PoseWithCovarianceStamped, queue_size=20) 

    #--- This event will be fired when the 'new goal' topic received data from Mobile ----------------------------------
    def callback_new_goal(self, msgReceived):
        point = self.convertMessToGoal(msgReceived)    #--- The first point in plan to delivery --
        #--- Add new Target to the delivery plan if it is not exsit ----------------------------------------------------
        if not self.deliveryPlan.isExist(point):
            #--- New goal is added to delivery plan --------------------------------------------------------------------
            self.deliveryPlan.add(point)
            point.info()				#--- Display new target info when accept to delivery plan ------
            ############################################################################################################
            msg = PoseWithCovarianceStamped()
            msg.header.frame_id = point.moveId 		#--- Set id of the MoveAction to the header.frame_id of the pose  
            msg.pose.pose = point.goal.target_pose.pose
            self.confirmGoal.publish(msg)	        #--- Confirm the received GOAL for android app -----------------
            ############################################################################################################
            #self.confirmGoal.publish(point.moveId)	#--- Confirm the received GOAL for android app -----------------

    #--- This event will be fired when the 'cancel goal' topic received data from Mobile -------------------------------
    def callback_calcel_goal(self, msgReceived): 
        #--- Add new Target to the delivery plan if it is not exsit ----------------------------------------------------
        self.deliveryPlan.cancel(msgReceived)
        #--- Will be deleted -------------------------------------------------------------------------------------------
        #rospy.loginfo('Delete MoveAction with ID: %s Succeed \n[ %s points in the delivery plan ]', msgReceived.data, self.deliveryPlan.size())

    #--- Ham dung cho muc dich doc trang thai cua action de hien thi tren man hinh [Test move_base/status]
    def callback_status(self, data):			#--- This event may be deleted in the future ###################
        #global curPlan 
        #--- Display the status of action on the screen if data was received 
        if len(data.status_list)>0:
            newGoalId = data.status_list[0].goal_id.id	#--- Get the first element of status_list array  
            newStatus = data.status_list[0].status
            #--- if get a new goal or new status then display on the screen 
            #if not curPlan.isExited(newGoalId, newStatus):
                #curPlan.goalId = newGoalId
                #curPlan.status = newStatus  
                #curPlan.displayInfo()

    #--- This function used to convert string message to MoveAction object  --------------------------------------------
    def convertMessToGoal(self, msg):
        temp= msg.data.split("#")
        p = PoseStamped()			      #--- New PoseStamped object to store coordinate 
        #---------------------------------------------------------------------------------------------------------------
        moveId = temp[0]
        receivedPerson = temp[1]
        #---------------------------------------------------------------------------------------------------------------
        p.pose.position.x = float(temp[2])
        p.pose.position.y = float(temp[3])
        p.pose.position.z = 0
        p.pose.orientation.x = 0
        p.pose.orientation.y = 0
        p.pose.orientation.z = 0
        p.pose.orientation.w = 1        
        #---------------------------------------------------------------------------------------------------------------
        point = MoveAction(moveId, receivedPerson, p, self.maxTimeToWait)    #--- The first point in plan to delivery --
        #---------------------------------------------------------------------------------------------------------------
        return point 

    #--- This method run over the time Robot works ---------------------------------------------------------------------
    def work(self):
        while not rospy.is_shutdown():
            if not self.deliveryPlan.isEmpty():	  #--- if the delivery plan is not empty then Move to target -----------
                target = self.deliveryPlan.delivery()
                
            self.rate.sleep()

#--- The main function in the user -------------------------------------------------------------------------------------
if __name__ == '__main__':
    try:
        #---------------------------------------------------------------------------------------------------------------
        rospy.loginfo('Neutral_node starting at: %s ... ', datetime.datetime.now())
        rospy.init_node('neutral_node', anonymous=True)

        #--- Initialize an delivery object with maximum time to wait for Goal reach at 5 minutes -----------------------
        run = delivery(300)
        run.work()

    except rospy.ROSInterruptException:
        pass
