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()
You can try to call self.move2goal()
directly in the update_goal()
function to be sure to received data before execution.