Send an action on callback of a subscriber

edit

Hello everyone,

I got stuck and I don't get how it's come from. I create a subscriber to some topic with a callback function. And in the callback I want to process something, see if a certain value (front) is lower than something, and if so send an action to a server. Everything work fine, but if the front value is lower than 0.3 during more than one time it's look like the client keeps all the calls as a stack and wait for the previous one to end and then send the next one. I try to setup some lock with self.ready_to_send to does not allow any other call to the server if the client does not get any result from the server, but it does not work. So if someone get a solution

#!/usr/bin/env python
import rospy
from std_msgs.msg import String, Int16
from slam_project_buggy.msg import AllSidesDistance
from sensor_msgs.msg import LaserScan
from slam_project_buggy.msg import setDirectionRobotAction, setDirectionRobotGoal
import slam_project_buggy
from actionlib import SimpleActionClient

limit_distance = 0.3
list_sides = ["front", "left", "back", "right"]

class clientSetDirectionRobot():
direction = ""

## Count the number of time that the listener 
buffer_loop =0
buffer_loop_max = 1
turnSide= ""
ready_to_send = True

def __init__(self):
    rospy.init_node('send_direction_client', anonymous=True)

    rospy.Subscriber("allMeanValues", AllSidesDistance, self.setDirection_cb)
    rospy.loginfo('Init Client')
    self.client = SimpleActionClient('set_direction', setDirectionRobotAction)

    # Waits until the action server has started up and started
    # listening for goals.
    # rospy.loginfo("Connect to server")
    self.client.wait_for_server()

def sendDirection(self, action):


    # Creates a goal to send to the action server.
    # # Sends the goal to the action server.
    # rospy.loginfo("Send the given goal")
    self.client.send_goal(action)

    # # Waits for the server to finish performing the action.
    self.client.wait_for_result()

    # # Prints out the result of executing the action
    # rospy.loginfo("Get the results")
    retour = self.client.get_result()  # A FibonacciResult

    return retour

# direction, angle, side
# direction: rotate, forward, backward, stop : str
# angle: -90 to 90 : int8
# side: left or right : str

# see setDirectionRobot in the action folder
def setDirection_cb(self, data):

    print("RESULT: %s", self.ready_to_send)

    if self.ready_to_send:
        try:
            front = data.listDistances[0].distance
            left  = data.listDistances[1].distance
            back  = data.listDistances[2].distance
            right = data.listDistances[3].distance
            rospy.loginfo('Front: '+ str(front))

            action_set = False

            if front < limit_distance and front != -1 :
                action = setDirectionRobotGoal(direction = "backward", 
                                               angle= 0, 
                                               side="", 
                                               backToPrevious=True)
                action_set = True

            #elif back < limit_distance and front != -1 :
            #   action_set = True
            #   action = setDirectionRobotGoal(direction = "forward", 
            #                                  angle= 0, 
            #                                  side="", 
            #                                  backToPrevious=True)

            if action_set:
                self.ready_to_send = False

                self.sendDirection(action)
                rospy.loginfo('Call the server, send action: %s', action.direction)

                self.ready_to_send = True

            #if front < limit_distance:

            #if right > limit_distance and left > limit_distance:
            #     action = setDirectionRobotGoal(direction = "backward", 
            #                                    angle= 0, 
            #                                    side="")


            #     elif right < limit_distance:
            #         action = setDirectionRobotGoal(direction = "rotate", 
            #                                    angle= 90,
            #                                    side="left")

            #     elif left < limit_distance:
            #         action = setDirectionRobotGoal(direction = "rotate", 
            #                                    angle= 90, 
            #                                    side="right")

        except IndexError:
            pass

if __name__ == '__main__':
    try:
        d = clientSetDirectionRobot()
        rospy.spin()
    except rospy.ROSInterruptException:
        print("Error")