Hello, I am working on the code of section 3 of the ROS2 in 5 Days Python course.
I managed to make the action server run and return the odometry values after one lap.
However, I am having a difficulty understanding why the feedback message current_total, that should show the updated distance travelled, was not being updated. I managed to make it work but I don’t understand why changing the position of time.sleep(1.0) would make any difference. First of all, below you can find the whole code of the action server that is running without any problem ( with the self.total_distance variable is being successfully updated in each loop):
import time
import rclpy
from rclpy.action import ActionServer
from rclpy.node import Node
from rclpy.executors import MultiThreadedExecutor
from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.qos import ReliabilityPolicy, DurabilityPolicy, QoSProfile
from custom_interface.action import OdomRecord
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Point
class OdomRecActionServer(Node):
def __init__(self):
super().__init__('odom_rec_action_server')
self.action_cbg = ReentrantCallbackGroup()
self.sub_cbg = self.action_cbg
self._action_server = ActionServer(
self, OdomRecord, 'record_odom', self.execute_callback, callback_group=self.action_cbg)
self.odom_subscriber = self.create_subscription(
Odometry,
'/odom',
self.odom_callback,
QoSProfile(depth=10, reliability=ReliabilityPolicy.BEST_EFFORT,
durability=DurabilityPolicy.VOLATILE),
callback_group=self.sub_cbg)
# variable used to store the current odometry message in odom_callback() (continuously updated)
self.last_odom = Point()
# variable used to store the initial odometry message in execute_callback() (NOT updated)
self.first_odom = Point()
# declaring variable used to store an array of odometry messages
self.odom_record = []
# variable used to store the updated distance travelled during the Actionc callback
self.total_distance = 0.0
# variables used to store the current x position for the feedback message
self.last_x = 0.0
# variables used to store the current y position for the feedback message
self.last_y = 0.0
def execute_callback(self, goal_handle):
self.get_logger().info('Executing goal...')
# creating the feedback message variable
feedback_msg = OdomRecord.Feedback()
feedback_msg.current_total = 0.0
# ensuring all variables that will be used below are reset
self.odom_record = []
self.last_x = 0.0
self.last_y = 0.0
self.total_distance = 0.0
# storing the first odometry values received (to be used as a reference later)
self.first_odom.x = self.last_odom.x
self.first_odom.y = self.last_odom.y
# iteration variable will be used to ensure that action server doesn't stop prematurely
iter = 0
while True:
# incrementing the number of iteration
iter += 1
# store current odometry measurement in an array of Point values (IN WHILE LOOP)
self.odom_record.append(self.last_odom)
# waiting 1 second before repeating the loop
# time.sleep(1.0)
# taking the last odometry measurment stored in the odom_record array
self.last_x = self.odom_record[-1].x
self.last_y = self.odom_record[-1].y
# waiting 1 second before repeating the loop
time.sleep(1.0)
# DEBUGGING CODE
self.get_logger().warn(f'last_x= {self.last_x}')
self.get_logger().warn(f'last_odom_x= {self.last_odom.x}')
self.get_logger().warn(f'last_y= {self.last_y}')
self.get_logger().warn(f'last_odom_y= {self.last_odom.y}')
# computing the distance travelled so far (IN WHILE LOOP)
self.total_distance = self.total_distance + self.dist_diff([self.last_x, self.last_y],
[self.last_odom.x, self.last_odom.y])
# Return as feedback the current value of self.total_distance (IN WHILE LOOP)
feedback_msg.current_total = self.total_distance
goal_handle.publish_feedback(feedback_msg)
# calculating the distance between the initial measurement and the current measurement
self.first_last_dist = self.dist_diff([self.last_odom.x, self.last_odom.y], [
self.first_odom.x, self.first_odom.y])
self.get_logger().info(
f"DISTANCE TO INITIAL POSITION= {self.first_last_dist}")
self.get_logger().warn(f'TOTAL DISTANCE= {self.total_distance}')
# check if initial position is reached again (iter>5 was added to ensure that the while loop doesn't end prematurely)
if self.first_last_dist < 0.05 and iter > 5:
self.get_logger().warn("INITIAL POSITION REACHED!")
break
# updating the goal_handle state to SUCCEED
goal_handle.succeed()
# returning the total list of odometry measurements
result = OdomRecord.Result()
result.list_of_odoms = self.odom_record
self.get_logger().info(
f'The list of odometry measurement is: {result.list_of_odoms}')
return result
def dist_diff(self, a, b):
return ((a[0]-b[0])**2 + (a[1]-b[1])**2)**0.5
def odom_callback(self, msg):
self.last_odom.x = msg.pose.pose.position.x
self.last_odom.y = msg.pose.pose.position.y
def main(args=None):
rclpy.init(args=args)
odom_record_server = OdomRecActionServer()
# Create a MultiThreadedExecutor
executor = MultiThreadedExecutor(num_threads=4)
# adding node to executor
executor.add_node(odom_record_server)
try:
# spin the executor
executor.spin()
finally:
executor.shutdown()
odom_record_server.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
An example output of the action server code above is:
user:~/ros2_ws$ ros2 run wall_follower odometry_recording
[INFO] [1677542142.680029059] [odom_rec_action_server]: Executing goal...
[WARN] [1677542143.682676880] [odom_rec_action_server]: last_x= -0.6726208989146514
[WARN] [1677542143.684019978] [odom_rec_action_server]: last_odom_x= -0.6782458260208939
[WARN] [1677542143.685634685] [odom_rec_action_server]: last_y= -0.005471049945289023
[WARN] [1677542143.686940758] [odom_rec_action_server]: last_odom_y= -0.05591538678318488
[INFO] [1677542143.688691758] [odom_rec_action_server]: DISTANCE TO INITIAL POSITION= 0.05075697906658393
[WARN] [1677542143.689649770] [odom_rec_action_server]: TOTAL DISTANCE= 0.05075697906658393
[WARN] [1677542144.743639571] [odom_rec_action_server]: last_x= -0.6782458260208939
[WARN] [1677542144.744663257] [odom_rec_action_server]: last_odom_x= -0.6854872295965918
[WARN] [1677542144.745574416] [odom_rec_action_server]: last_y= -0.05591538678318488
[WARN] [1677542144.747110056] [odom_rec_action_server]: last_odom_y= -0.11875939360816222
[INFO] [1677542144.750488593] [odom_rec_action_server]: DISTANCE TO INITIAL POSITION= 0.11401662718697787
[WARN] [1677542144.751451130] [odom_rec_action_server]: TOTAL DISTANCE= 0.11401681719128931
[WARN] [1677542145.755498129] [odom_rec_action_server]: last_x= -0.6858646325452228
[WARN] [1677542145.757089662] [odom_rec_action_server]: last_odom_x= -0.6932108318615994
[WARN] [1677542145.758061295] [odom_rec_action_server]: last_y= -0.12204490204340702
[WARN] [1677542145.759114212] [odom_rec_action_server]: last_odom_y= -0.1846577651779565
[INFO] [1677542145.764101743] [odom_rec_action_server]: DISTANCE TO INITIAL POSITION= 0.18036580677787264
[WARN] [1677542145.765368661] [odom_rec_action_server]: TOTAL DISTANCE= 0.17705916228014018
[WARN] [1677542146.767275405] [odom_rec_action_server]: last_x= -0.6936142100484254
[WARN] [1677542146.768088894] [odom_rec_action_server]: last_odom_x= -0.7003341625677505
[WARN] [1677542146.768774104] [odom_rec_action_server]: last_y= -0.18802165000832768
[WARN] [1677542146.769409807] [odom_rec_action_server]: last_odom_y= -0.2442640375504716
[INFO] [1677542146.770327432] [odom_rec_action_server]: DISTANCE TO INITIAL POSITION= 0.2403957485308654
[WARN] [1677542146.772358849] [odom_rec_action_server]: TOTAL DISTANCE= 0.23370158382356193
[WARN] [1677542147.773864813] [odom_rec_action_server]: last_x= -0.7003341625677505
[WARN] [1677542147.774726841] [odom_rec_action_server]: last_odom_x= -0.7088579512904147
[WARN] [1677542147.775295985] [odom_rec_action_server]: last_y= -0.2442640375504716
[WARN] [1677542147.775939050] [odom_rec_action_server]: last_odom_y= -0.31336493297064044
[INFO] [1677542147.776589871] [odom_rec_action_server]: DISTANCE TO INITIAL POSITION= 0.3100189787243881
[WARN] [1677542147.777148386] [odom_rec_action_server]: TOTAL DISTANCE= 0.3033262110904833
[WARN] [1677542148.779990334] [odom_rec_action_server]: last_x= -0.7088579512904147
[WARN] [1677542148.780702700] [odom_rec_action_server]: last_odom_x= -0.7162730189917992
[WARN] [1677542148.781630045] [odom_rec_action_server]: last_y= -0.31336493297064044
[WARN] [1677542148.782457153] [odom_rec_action_server]: last_odom_y= -0.37273631540122176
[INFO] [1677542148.783304930] [odom_rec_action_server]: DISTANCE TO INITIAL POSITION= 0.3698503518960696
[WARN] [1677542148.784117876] [odom_rec_action_server]: TOTAL DISTANCE= 0.3631588466728681
[WARN] [1677542149.843405040] [odom_rec_action_server]: last_x= -0.7162730189917992
[WARN] [1677542149.845916246] [odom_rec_action_server]: last_odom_x= -0.7234921270999027
[WARN] [1677542149.846949589] [odom_rec_action_server]: last_y= -0.37273631540122176
[WARN] [1677542149.848319269] [odom_rec_action_server]: last_odom_y= -0.4321481234285962
[INFO] [1677542149.849860272] [odom_rec_action_server]: DISTANCE TO INITIAL POSITION= 0.4297489851025187
[WARN] [1677542149.851281749] [odom_rec_action_server]: TOTAL DISTANCE= 0.4230598490608686
[WARN] [1677542150.853366177] [odom_rec_action_server]: last_x= -0.7239128589119578
[WARN] [1677542150.854216207] [odom_rec_action_server]: last_odom_x= -0.7321036986772314
[WARN] [1677542150.855059434] [odom_rec_action_server]: last_y= -0.4321481234285962
[WARN] [1677542150.855906459] [odom_rec_action_server]: last_odom_y= -0.4947409855985233
[INFO] [1677542150.858320416] [odom_rec_action_server]: DISTANCE TO INITIAL POSITION= 0.49287247174265597
[WARN] [1677542150.859902941] [odom_rec_action_server]: TOTAL DISTANCE= 0.486186358949218
[WARN] [1677542151.861502007] [odom_rec_action_server]: last_x= -0.7321036986772314
[WARN] [1677542151.862484768] [odom_rec_action_server]: last_odom_x= -0.7383394251759293
[WARN] [1677542151.863722489] [odom_rec_action_server]: last_y= -0.4947409855985233
[WARN] [1677542151.864752795] [odom_rec_action_server]: last_odom_y= -0.5480838666223671
[INFO] [1677542151.865673514] [odom_rec_action_server]: DISTANCE TO INITIAL POSITION= 0.5465780763223006
However, I was having a problem when time.sleep(1.0) is uncommented just after appending the current odometry value to the self.odom_record array, as shown below. In this case, the total_distance variable would not get updated.
while True:
# incrementing the number of iteration
iter += 1
# store current odometry measurement in an array of Point values (IN WHILE LOOP)
self.odom_record.append(self.last_odom)
# waiting 1 second before repeating the loop
time.sleep(1.0)
# taking the last odometry measurment stored in the odom_record array
self.last_x = self.odom_record[-1].x
self.last_y = self.odom_record[-1].y
# waiting 1 second before repeating the loop
# time.sleep(1.0)
# DEBUGGING CODE
self.get_logger().warn(f'last_x= {self.last_x}')
self.get_logger().warn(f'last_odom_x= {self.last_odom.x}')
self.get_logger().warn(f'last_y= {self.last_y}')
self.get_logger().warn(f'last_odom_y= {self.last_odom.y}')
# computing the distance travelled so far (IN WHILE LOOP)
self.total_distance = self.total_distance + self.dist_diff([self.last_x, self.last_y],
[self.last_odom.x, self.last_odom.y])
And the output message coming from the action server in this case would be:
user:~/ros2_ws$ ros2 run wall_follower odometry_recording
[INFO] [1677541290.174260067] [odom_rec_action_server]: Executing goal...
[WARN] [1677541291.177609647] [odom_rec_action_server]: last_x= 0.4794518750645817
[WARN] [1677541291.178560632] [odom_rec_action_server]: last_odom_x= 0.4794518750645817
[WARN] [1677541291.179550269] [odom_rec_action_server]: last_y= 0.57807118235036
[WARN] [1677541291.180583174] [odom_rec_action_server]: last_odom_y= 0.57807118235036
[INFO] [1677541291.181561718] [odom_rec_action_server]: DISTANCE TO INITIAL POSITION= 0.052386824684167214
[WARN] [1677541291.182749057] [odom_rec_action_server]: TOTAL DISTANCE= 0.0
[WARN] [1677541292.187788291] [odom_rec_action_server]: last_x= 0.42584469004918685
[WARN] [1677541292.188857163] [odom_rec_action_server]: last_odom_x= 0.42584469004918685
[WARN] [1677541292.189895429] [odom_rec_action_server]: last_y= 0.6043207953602485
[WARN] [1677541292.191020197] [odom_rec_action_server]: last_odom_y= 0.6043207953602485
[INFO] [1677541292.192101484] [odom_rec_action_server]: DISTANCE TO INITIAL POSITION= 0.11206327548787373
[WARN] [1677541292.193148400] [odom_rec_action_server]: TOTAL DISTANCE= 0.0
[WARN] [1677541293.197031827] [odom_rec_action_server]: last_x= 0.3751443507044991
[WARN] [1677541293.197949521] [odom_rec_action_server]: last_odom_x= 0.3751443507044991
[WARN] [1677541293.198882016] [odom_rec_action_server]: last_y= 0.6291292805008104
[WARN] [1677541293.243135607] [odom_rec_action_server]: last_odom_y= 0.6291292805008104
[INFO] [1677541293.244259209] [odom_rec_action_server]: DISTANCE TO INITIAL POSITION= 0.16850427726454678
[WARN] [1677541293.245197671] [odom_rec_action_server]: TOTAL DISTANCE= 0.0
[WARN] [1677541294.250472737] [odom_rec_action_server]: last_x= 0.3360427235019496
[WARN] [1677541294.251380302] [odom_rec_action_server]: last_odom_x= 0.3360427235019496
[WARN] [1677541294.252882512] [odom_rec_action_server]: last_y= 0.6467394970644721
[WARN] [1677541294.253723770] [odom_rec_action_server]: last_odom_y= 0.6467394970644721
[INFO] [1677541294.254608342] [odom_rec_action_server]: DISTANCE TO INITIAL POSITION= 0.21137963584037903
[WARN] [1677541294.256587080] [odom_rec_action_server]: TOTAL DISTANCE= 0.0
[WARN] [1677541295.260006257] [odom_rec_action_server]: last_x= 0.2758662231249633
[WARN] [1677541295.261110346] [odom_rec_action_server]: last_odom_x= 0.2758662231249633
[WARN] [1677541295.262126842] [odom_rec_action_server]: last_y= 0.664353171040243
[WARN] [1677541295.269069133] [odom_rec_action_server]: last_odom_y= 0.664353171040243
[INFO] [1677541295.274211619] [odom_rec_action_server]: DISTANCE TO INITIAL POSITION= 0.27348875541996887
[WARN] [1677541295.275473234] [odom_rec_action_server]: TOTAL DISTANCE= 0.0
[WARN] [1677541296.280095787] [odom_rec_action_server]: last_x= 0.21176779545284313
[WARN] [1677541296.281527472] [odom_rec_action_server]: last_odom_x= 0.21176779545284313
[WARN] [1677541296.283060991] [odom_rec_action_server]: last_y= 0.6773592530578566
[WARN] [1677541296.284254196] [odom_rec_action_server]: last_odom_y= 0.6773592530578566
[INFO] [1677541296.285222494] [odom_rec_action_server]: DISTANCE TO INITIAL POSITION= 0.33778236778012233
[WARN] [1677541296.286330123] [odom_rec_action_server]: TOTAL DISTANCE= 0.0
[WARN] [1677541297.291306712] [odom_rec_action_server]: last_x= 0.15981497577903334
[WARN] [1677541297.292156922] [odom_rec_action_server]: last_odom_x= 0.15981497577903334
[WARN] [1677541297.292967509] [odom_rec_action_server]: last_y= 0.6817164950071584
[WARN] [1677541297.293770558] [odom_rec_action_server]: last_odom_y= 0.6817164950071584
[INFO] [1677541297.294786832] [odom_rec_action_server]: DISTANCE TO INITIAL POSITION= 0.3881245862126242
[WARN] [1677541297.295653759] [odom_rec_action_server]: TOTAL DISTANCE= 0.0
[WARN] [1677541298.297791043] [odom_rec_action_server]: last_x= 0.10111418690491109
[WARN] [1677541298.347995525] [odom_rec_action_server]: last_odom_x= 0.10111418690491109
[WARN] [1677541298.349073971] [odom_rec_action_server]: last_y= 0.6800319826982555
[WARN] [1677541298.350081239] [odom_rec_action_server]: last_odom_y= 0.6800319826982555
[INFO] [1677541298.352178493] [odom_rec_action_server]: DISTANCE TO INITIAL POSITION= 0.4436161928511153
[WARN] [1677541298.358890878] [odom_rec_action_server]: TOTAL DISTANCE= 0.0
[WARN] [1677541299.361411285] [odom_rec_action_server]: last_x= 0.0428699295252698
[WARN] [1677541299.362339448] [odom_rec_action_server]: last_odom_x= 0.0428699295252698
[WARN] [1677541299.381338605] [odom_rec_action_server]: last_y= 0.6714783661723073
[WARN] [1677541299.412316286] [odom_rec_action_server]: last_odom_y= 0.6708092025689542
[INFO] [1677541299.413566589] [odom_rec_action_server]: DISTANCE TO INITIAL POSITION= 0.5006727061408018
[WARN] [1677541299.417207248] [odom_rec_action_server]: TOTAL DISTANCE= 0.003208540194815803
[WARN] [1677541300.443477337] [odom_rec_action_server]: last_x= -0.013942754725924844
[WARN] [1677541300.445374861] [odom_rec_action_server]: last_odom_x= -0.013942754725924844
[WARN] [1677541300.447539358] [odom_rec_action_server]: last_y= 0.6559830908993256
[WARN] [1677541300.449335970] [odom_rec_action_server]: last_odom_y= 0.6559830908993256
[INFO] [1677541300.451842796] [odom_rec_action_server]: DISTANCE TO INITIAL POSITION= 0.5501901124997405
[WARN] [1677541300.454085836] [odom_rec_action_server]: TOTAL DISTANCE= 0.003208540194815803
[WARN] [1677541301.458055579] [odom_rec_action_server]: last_x= -0.07100828279412819
[WARN] [1677541301.459001430] [odom_rec_action_server]: last_odom_x= -0.07100828279412819
[WARN] [1677541301.459814731] [odom_rec_action_server]: last_y= 0.632506548078621
[WARN] [1677541301.460624726] [odom_rec_action_server]: last_odom_y= 0.632506548078621
[INFO] [1677541301.462801560] [odom_rec_action_server]: DISTANCE TO INITIAL POSITION= 0.6029935646591323
Can someone tell me why does this happen ? I thought that the position of time.sleep(1.0) whether it
is added before or after updating the values of self.last_x and self.last_y wouldn’t make any difference, since in any case, self.last_x and self.last_y are updated from the last value stored in the self.odom_record array.
Thank you for your help.
It is in the following line of code when I store the first odometry measurement: