Send an action on callback of a subscriber

asked 2017-07-08 20:39:25 -0500

Dwighthaul gravatar image

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")
edit retag flag offensive close merge delete

Comments

it's look like the client keeps all the calls as a stack and wait for the previous one to end

yes, that's how a regular CallbackQueue works. All incoming events (ie: msgs) are stored in the queue (fifo) and get processed one-by-one.

gvdhoorn gravatar imagegvdhoorn ( 2017-07-09 04:57:33 -0500 )edit

Ok, I get it, but do you know how to don't have this? Because I try to lock this with self.ready_to_send to not enter in the function and don't call the "sendDirection" function and send data to the server on. Or should I send the way.Perhaps I should change the way to call my callback function,no?

Dwighthaul gravatar imageDwighthaul ( 2017-07-09 09:27:12 -0500 )edit

PS: I only add queue_size=1 when I start my subscriber so it will not stack the calls. Thanks Gvdhoorn to put me in the right way!

Dwighthaul gravatar imageDwighthaul ( 2017-07-09 09:52:09 -0500 )edit