Search code examples
pythonrosgazebo-simu

Message received by subscriber only after input() command


My gazebo model can move to a given goal destination. so far the goal was given by user input as such:

goal_pose.x = float(input("Set your x goal: "))
goal_pose.y = float(input("Set your y goal: "))

I want to send the goal destination through a node and not by user input. so I created another node that publish some random goal destination.

The problem is that when i try to subscribe to that node, it only subscribe after i use input() command.

the code that doesnt work:

        print("self.goal , is:") 
        print(self.goal_pose.x, self.goal_pose.y)
        print("self.goal , is:")
        print(self.goal_pose.x, self.goal_pose.y)

output:

self.goal , is:
(0.0, 0.0)
self.goal , is:
(0.0, 0.0)

the code that does work:

        print("self.goal , is:")
        print(self.goal_pose.x, self.goal_pose.y)
        input("type something and press enter: ")
        print("self.goal , is:")
        print(self.goal_pose.x, self.goal_pose.y)

output:

self.goal , is:
(0.0, 0.0)
type something and press enter: 22
self.goal , is:
(5.0, 6.0)

I would like to know, what is the reason for this behavior and how to fix it?

Attached some essentials of the code:

class oferbot_controller:

    def __init__(self):
        rospy.init_node(const.MOVE_NODE_NAME, anonymous=True)
        self.velocity_publisher = rospy.Publisher(const.CMD_VEL_TOPIC, Twist, queue_size=10)
        self.odom_subscriber = rospy.Subscriber(const.ODOMETRY_TOPIC,Odometry, self.update_odom)
        self.goal_subscriber = rospy.Subscriber(const.GOAL_POSE_TOPIC,Pose,self.update_goal)
        self.odom = Odometry()
        self.goal_pose = Pose()
        self.vel_msg = Twist()
        self.teta = float()
        self.rate = rospy.Rate(10)
        
        


    def update_goal(self,data): 
        self.goal_pose = data
        self.goal_pose.x = round(self.goal_pose.x, 4)
        self.goal_pose.y = round(self.goal_pose.y, 4)

    
    
    def move2goal(self):
        
        
        print("self.goal , is:")
        print(self.goal_pose.x, self.goal_pose.y)
        input("type something and press enter: ")
        print("self.goal , is:")
        print(self.goal_pose.x, self.goal_pose.y)


        while calc.euclidean_distance(self.odom,self.goal_pose) >= const.DISTANCE_TOLERANCE:
            ## Make the robot move accordingly ##

        # Stopping our robot after the movement is over.
        self.stop_the_bot()
        rospy.spin()

if __name__ == '__main__':
    try:
        x = oferbot_controller()
        x.move2goal()
    except rospy.ROSInterruptException:
        pass

the publisher node runs in a different terminal. the publisher code:

rospy.init_node('point_goal_publisher')

point_goal_pub = rospy.Publisher(const.GOAL_POSE_TOPIC, Pose, queue_size=10)
point_goal = Pose()
point_goal.x = const.POINT_X
point_goal.y = const.POINT_Y
rate = rospy.Rate(10)
print("Publishing point goal:")
print("x goal = ", point_goal.x ,"y goal = ", point_goal.y)
while not rospy.is_shutdown():
    point_goal_pub.publish(point_goal)
    rate.sleep()

Solution

  • You can try to call self.move2goal() directly in the update_goal() function to be sure to received data before execution.