How to deal with multiple subscribers and callbacks for different topics in rospy?
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)