Hello. I am currently doing the quiz 5 and I’m not able to get the odom message all the time.
I mean, when I echo the /odom topic by terminal, I can see the messages updating all the time.
When I try to subscribe by python script, I only receive the message each ~10 seconds.
import rclpy
from rclpy.action import ActionServer
from rclpy.node import Node
from actions_quiz_msg.action import Distance
from std_msgs.msg import Float64
from nav_msgs.msg import Odometry
from rclpy.qos import ReliabilityPolicy, QoSProfile
import math
import time
class MyActionServer(Node):
def __init__(self):
super().__init__('my_action_server')
self._action_server = ActionServer(self, Distance, 'distance_as',self.execute_callback)
self.cmd = Float64()
self.x_past = 0.0
self.y_past = 0.0
self.x_act = 0.0
self.y_act = 0.0
self.calculated_distance = 0.0
self.start = True
self.publisher_ = self.create_publisher(Float64, 'total_distance', 10)
self.subscriber = self.create_subscription(Odometry, '/odom', self.odom_callback, QoSProfile(depth=10, reliability=ReliabilityPolicy.RELIABLE))
def odom_callback(self, msg):
self.x_act = msg.pose.pose.position.x
self.y_act = msg.pose.pose.position.y
print ("Message received: ", self.x_act)
def execute_callback(self, goal_handle):
self.get_logger().info('Executing goal...')
feedback_msg = Distance.Feedback()
for i in range(0, goal_handle.request.seconds):
self.get_logger().info('X Actual: {0}, Y Actual: {1}'.format(str(self.x_act), str(self.y_act)))
#self.get_logger().info('X Pasada: {0}, Y Pasada: {1}'.format(str(self.x_past), str(self.y_past)))
self.get_logger().info('Executing goal...')
if (self.start):
self.calculated_distance = 0.0
self.x_past = self.x_act
self.y_past = self.y_act
self.start = False
self.calculated_distance = self.calculated_distance + math.sqrt(math.pow(self.x_act - self.x_past,2) + math.pow(self.y_act - self.y_past,2))
self.cmd.data = self.calculated_distance
feedback_msg.current_dist = self.calculated_distance
self.get_logger().info('Feedback: Calculated distance: {0} '.format(str(self.calculated_distance)))
goal_handle.publish_feedback(feedback_msg)
self.publisher_.publish(self.cmd)
self.x_past = self.x_act
self.y_past = self.y_act
time.sleep(1)
goal_handle.succeed()
result = Distance.Result()
result.status = True
result.total_dist = self.calculated_distance
self.get_logger().info('Result: Status:{0}, Total distance: {1}'.format(str(result.status), str(result.total_dist)))
return result
def main(args=None):
rclpy.init(args=args)
my_action_server = MyActionServer()
rclpy.spin(my_action_server)
if __name__ == '__main__':
main()
output of ros2 topic info -v /odom
What am I doing wrong?
Thank you
