[message_filters, Noetic, move_base]The callback function is not reached by message_filters.

edit

I am trying to use message_filters to use GPS and move_base status topic at the same time.

Below is the current test-cord.

No error is output, but the robot does not move because the callback function is not reached.

Do you know what could be causing this?

#! /usr/bin/env python3

import rospy
from geometry_msgs.msg import PoseStamped
#import tf
from sensor_msgs.msg import NavSatFix
import numpy as np
import math
import time
from geometry_msgs.msg import Quaternion
from calcxy import calc_xy
from actionlib_msgs.msg import GoalStatusArray
import message_filters

# Define publisher, subscriber topic name
DEFAULT_SUBSCRIBER_TOPIC_0 = "/ublox/fix" 
DEFAULT_SUBSCRIBER_TOPIC_1 = "/move_base/status" 


class Goal:

    #initialization
    def __init__(self):
        rospy.init_node('message_filters_test', anonymous=True)
        self.ps_pub = rospy.Publisher('move_base_simple/goal', PoseStamped, queue_size=1)

        # From ROS parameter (set in launch file)
        subscriber_topic_0 = rospy.get_param("~subscriber_topic_0", DEFAULT_SUBSCRIBER_TOPIC_0)
        subscriber_topic_1 = rospy.get_param("~subscriber_topic_1", DEFAULT_SUBSCRIBER_TOPIC_1)

        # Creating subscriber, subscribe (recieve) from topic
        # message_filters version
        self.sub_0 = message_filters.Subscriber(subscriber_topic_0, NavSatFix)
        self.sub_1 = message_filters.Subscriber(subscriber_topic_1, GoalStatusArray)

        # message_filters
        fps = 100
        delay = 1/fps*0.5
        ts = message_filters.ApproximateTimeSynchronizer([self.sub_0, self.sub_1], 10, delay)
        ts.registerCallback(self.callback)

        print("Goal.__init__")

        self.time = 0


    def callback(self, gps, robot):

        print("Goal.callback")

        if self.time == 0:
            gps_status = gps.status.status
            print("GPS_STATUS")
            print(gps_status)
            if gps.status.status == -1:
                self.time = self.time + 1
                """
                GPS Settings
                """
            else:
                pass 

        elif self.time == 1:
            self.time = self.time + 1
            """
            calculation
            """

            # mone_base_simple/goal
            rospy.sleep(1.0)

            now = rospy.Time.now()
            goal_point = PoseStamped()

            goal_point.pose.position.x = 0.5
            goal_point.pose.position.y = 0
            goal_point.pose.position.z = 0.0
            goal_point.pose.orientation.w = 1.0
            goal_point.header.stamp = now 
            goal_point.header.frame_id = 'map'

            self.ps_pub.publish(goal_point)

        elif self.time == 2:

            len_list = robot.status_list
            if len(len_list) == 1:
                robot_status = robot.status_list[0].status
                print("ROBOT_STATUS")
                print(robot_status)
                if robot_status == 3:
                    self.time = self.time + 1
                    print("SUCCESS")
                else :
                    pass
            else :
                pass

        elif self.time == 3:

            self.time = self.time + 1

            # mone_base_simple/goal
            rospy.sleep(1.0)

            now = rospy.Time.now()
            goal_point = PoseStamped()

            goal_point.pose.position.x = 1.5
            goal_point.pose.position.y = 0
            goal_point.pose.position.z = 0.0
            goal_point.pose.orientation.w = 1.0
            goal_point.header.stamp = now 
            goal_point.header.frame_id = 'map'

            self.ps_pub.publish(goal_point)

        else:
            pass


        #robot_status=robot.header.seq
        #print(robot_status)


if __name__ == '__main__':


    try:
        set_goal_node = Goal()
        print("__name__")
        rospy.spin()
    except rospy.ROSInterruptException: pass

output

Goal.__init__
__name__

The output shows that the callback function has not been reached.

Do you know how to solve this?

Thank you.