Ask Your Question
0

How to deal with multiple subscribers and callbacks for different topics in rospy?

asked 2020-04-29 12:22:54 -0500

mkb_10062949 gravatar image

Hello I have a code snippet here which 8 subscribers for 8 different message types. Earlier I used to have only four subscribersand it used to run very fast but with 8 subscribers the subscription of messages is really slow. I am familial message_filters but everywhere I see it only shows example for message types which are same. But in my case I have different message types which needs to be stored in different variables. The variables need to be used later for other calculations. How can I do it much efficiently and really fast. The subscriber of each tpoic is publishing at a rate of 1000 hz. The subscribers are the ones slow here.

class Robot1(Robot):
    def __init__(self, beTa):
        super(Robot1, self).__init__()
        rospy.init_node(self.__class__.__name__)
        self.action_pub = rospy.Publisher("robot1/arm_controller/command", JointTrajectory, queue_size=1)
        self.gripper_pub = rospy.Publisher("robot1/gripper_controller/command", JointTrajectory, queue_size=1)
        rospy.Subscriber("robot1/arm_controller/state", JointTrajectoryControllerState, self.joint_angles_call_r1)
        rospy.Subscriber("robot2/arm_controller/state", JointTrajectoryControllerState, self.joint_angles_call_r2)
        rospy.Subscriber("robot1/gripper_controller/state", JointTrajectoryControllerState, self.gripper_joint_r1)
        rospy.Subscriber("robot1/ft_sensor/raw", WrenchStamped, self.observation_callback_collision)
        rospy.Subscriber("robot1/ft_sensor/gripper_r", WrenchStamped, self.gripper_callback_right_r1)
        rospy.Subscriber("robot1/ft_sensor/gripper_l", WrenchStamped, self.gripper_callback_left_r1)
        rospy.Subscriber("robot2/ft_sensor/gripper_r", WrenchStamped, self.gripper_callback_right_r2)
        rospy.Subscriber("robot2/ft_sensor/gripper_l", WrenchStamped, self.gripper_callback_left_r2)
        #######################################################################################
        self.beTa = beTa
        self.robot_name = 'R1'
        self.targetPosition = self.comm_tp1
        self.target_orientation = self.comm_to1
        self.positions = [self.comm_tp1]
        self.orientations = [self.comm_to1]
        self.reset_joints = [1.5708,-1.5708,0, 0, 1.5708,0]

    def joint_angles_call_r1(self, message):
        self.message1 = message
        self.curr_joints = ut_ur5.processObservations(self.message1)
        #rospy.sleep(0.0005)

    def joint_angles_call_r2(self, message):
        self.message2 = message
        #rospy.sleep(0.0005)

    def gripper_joint_r1(self, pos_msg):
        self.gripper_pos_msg = pos_msg
        #rospy.sleep(0.0005)

    def observation_callback_collision(self, FT):
        self._coll = FT
        #rospy.sleep(0.0005)

    def gripper_callback_left_r1(self, FT):
        self.left_gripper_r1 = np.array(FT.wrench.force.x)
        #rospy.sleep(0.0005)

    def gripper_callback_right_r1(self, FT):
        self.right_gripper_r1 = np.array(FT.wrench.force.x)
        #rospy.sleep(0.0005)

    def gripper_callback_left_r2(self, FT):
        self.left_gripper_r2 = np.array(FT.wrench.force.x)
        #rospy.sleep(0.0005)

    def gripper_callback_right_r2(self, FT):
        self.right_gripper_r2 = np.array(FT.wrench.force.x)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2020-04-29 14:14:29 -0500

Mark Rose gravatar image

_Each_ publisher is publishing at 1000Hz? That's your main problem. You have 8000 messages/second. Python is not truly multithreaded. Instead, there is a _global interpreter lock_ that causes only one thread to run at a time. It's like your 8 subscribers are sharing a single CPU. At a total message rate of 8KHz, you must process each message in only 125 microseconds, including any thread switching and unmarshaling overhead. Python is not designed to handle that kind of load.

You're better off reducing the publishing rate to something more reasonable, say 100Hz. Try that and see if your Python code behaves reasonably again.

edit flag offensive delete link more

Comments

Okay got it, I modified the publisher rate and I see a significant improvement. Thank you so much!

mkb_10062949 gravatar image mkb_10062949  ( 2020-04-29 14:56:15 -0500 )edit

Is there a way to parallelize the code using Multiprocessing module to bypass the GIL to use multi cores with ROS?

mkb_10062949 gravatar image mkb_10062949  ( 2020-04-29 14:57:24 -0500 )edit

Maybe, but remember that the Multiprocessing module spawns new processes, and coordination between processes uses interprocess communication under the hood, so that introduces new overhead. It depends on your actual application whether you will gain much processing speed, since you're still programming in Python (which is, generally, slow). If you really need the throughput you tried to achieve, I imagine you would have to switch to C/C++. (But I tend to doubt you need 1KHz messaging to control your robot. Even low-level PID control of a motor is usually fine at 50Hz, or even less, on a microcontroller, of course.)

Also be aware that you're never going to be able to do truly real-time processing in ROS1, because of the vagaries of multiprocessing in non-real-time operating systems. If you need real-time control, you may need a co-processor (not running ROS).

Mark Rose gravatar image Mark Rose  ( 2020-04-30 17:41:34 -0500 )edit

Okay, I have actually asked a question in this regard with a code snippet here, https://answers.ros.org/question/3506.... I have also mentioned a problem that I face with rospy. Would be great if you can answer it.

mkb_10062949 gravatar image mkb_10062949  ( 2020-05-01 02:46:49 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

3 followers

Stats

Asked: 2020-04-29 12:22:54 -0500

Seen: 749 times

Last updated: Apr 29 '20