I think I am having an issue with the grading bot. tried to do the action quiz, and I thought I did everything correctly. However, I am getting the feedback below from the grading bot:
I am not sure what I am doing wrong since I am extracting the XY coordinates from the odometry message and calculating the distance travelled, using the formula : √[(x2 – x1)^2 + (y2 – y1)^2].
Can someone tell me what I am doing wrong ?
You can find my action server code below:
import time
import rclpy
import numpy as np
from rclpy.action import ActionServer
from rclpy.node import Node
from rclpy.qos import ReliabilityPolicy, QoSProfile
from actions_quiz_msg.action import Distance
from nav_msgs.msg import Odometry
from std_msgs.msg import Float64
from geometry_msgs.msg import Point
# Import the libraries to use executors and callback groups
from rclpy.callback_groups import ReentrantCallbackGroup, MutuallyExclusiveCallbackGroup
from rclpy.executors import MultiThreadedExecutor, SingleThreadedExecutor
class QuizActionServer(Node):
def __init__(self):
super().__init__('quiz_action_server')
self.group1 = MutuallyExclusiveCallbackGroup()
self.group2 = MutuallyExclusiveCallbackGroup()
self._action_server = ActionServer(
self, Distance, 'distance_as', self.execute_callback, callback_group=self.group1)
self.subscriber_ = self.create_subscription(
Odometry, '/odom', self.odometry_callback,
QoSProfile(depth=10, reliability=ReliabilityPolicy.RELIABLE), callback_group=self.group2)
self.publisher_ = self.create_publisher(Float64, '/total_distance', 10)
self.stored_init_meas = False
# publisher variable used to send message to the /total_distance topic
self.pub_var = Float64()
# used to store initial measurement
self.xy_0 = Point()
# used to store current measurement
self.xy_curr = Point()
def odometry_callback(self, msg):
# storing the initial measurement before setting the goal
if self.stored_init_meas is False:
self.xy_0 = msg.pose.pose.position
# needs to be set back to False at the end of the program
self.stored_init_meas = True
# storing current measurement
self.xy_curr = msg.pose.pose.position
# self.get_logger().info('Odometry measurements received...')
def execute_callback(self, goal_handle):
self.get_logger().info('Initiating goal ...')
feedback_msg = Distance.Feedback()
for i in range(goal_handle.request.seconds):
# calculating current distance travelled
curr_dist = np.sqrt((self.xy_curr.x-self.xy_0.x)
** 2 + (self.xy_curr.y-self.xy_0.y)**2)
# storing current distance in the feedback part of the action message
feedback_msg.current_dist = curr_dist
# publishing current distance to the /total_distance topic
self.pub_var.data = curr_dist
self.publisher_.publish(self.pub_var)
# publishing current distance as feedback of the Action server
goal_handle.publish_feedback(feedback_msg)
# waiting for 1 second
time.sleep(1.0)
# updating the goal_handle state to SUCCEED:
goal_handle.succeed()
# set Boolean of initial measurement back to False:
self.stored_init_meas = False
# returning the total distance in the result part of the action message
result = Distance.Result()
result.status = True
result.total_dist = curr_dist
self.get_logger().info(
f'The total distance travelled is: {result.total_dist}')
return result
def main(args=None):
rclpy.init(args=args)
my_quiz_server = QuizActionServer()
# Create a MultiThreadedExecutor
executor = MultiThreadedExecutor(num_threads=3)
# adding node to executor
executor.add_node(my_quiz_server)
try:
# spin the executor
executor.spin()
finally:
executor.shutdown()
my_quiz_server.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
After going through your code, I see that there is nothing logically wrong. Your implementation is also correct. Distance calculation logic is also correct.
What you can do is, post the outputs of the /distance_as from the terminal. I believe there might be a value mismatch with the GradeBot’s distance check. Post the outputs as code-block.
Regards,
Girish
EDIT:
Are you sure that your node name is correct?
Should it be quiz_action_server or actions_quiz_server or actions_quiz_server_node or something else?
If I am reading it correctly, the odometry_callback would set the initial and current values as the same values then the execute_callback would run and the current position values would never update as all of the calculations are done in a single call of the function(edit: method) .
Yes, I double checked that the name of the launch file is " actions_quiz_server.launch.py".
I am not sure what’s going on. I sent the same goal request to the action server 6 times while publishing a linear velocity of 0.05 using the teleop_keyboard node. I was expecting to get the same result 6 times. However, I got the following output for my action server :
user:~/ros2_ws$ ros2 launch actions_quiz actions_quiz_server.launch.py
[INFO] [launch]: All log files can be found below /home/user/.ros/log/2023-02-21-18-03-02-854420-2_xterm-31126
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [action_server_node-1]: process started with pid [31127]
[action_server_node-1] [INFO] [1677002587.174362587] [quiz_action_server]: Initiating goal ...
[action_server_node-1] [INFO] [1677002607.197233158] [quiz_action_server]: The total distance travelled is: 1.0411060172229492
[action_server_node-1] [INFO] [1677002615.461181875] [quiz_action_server]: Initiating goal ...
[action_server_node-1] [INFO] [1677002635.485812199] [quiz_action_server]: The total distance travelled is: 1.2584984505198697
[action_server_node-1] [INFO] [1677002641.174372988] [quiz_action_server]: Initiating goal ...
[action_server_node-1] [INFO] [1677002661.199191762] [quiz_action_server]: The total distance travelled is: 1.1294216775530124
[action_server_node-1] [INFO] [1677002689.201378488] [quiz_action_server]: Initiating goal ...
[action_server_node-1] [INFO] [1677002709.226459259] [quiz_action_server]: The total distance travelled is: 2.160337341760508
[action_server_node-1] [INFO] [1677002715.128386760] [quiz_action_server]: Initiating goal ...
[action_server_node-1] [INFO] [1677002735.157327686] [quiz_action_server]: The total distance travelled is: 0.9103308345484348
[action_server_node-1] [INFO] [1677002740.250997108] [quiz_action_server]: Initiating goal ...
[action_server_node-1] [INFO] [1677002760.280370850] [quiz_action_server]: The total distance travelled is: 0.9290129897188939
You can also find below the output of the action client node to see the feedback message:
user:~/ros2_ws$ ros2 launch actions_quiz actions_quiz_client.launch.py
[INFO] [launch]: All log files can be found below /home/user/.ros/log/2023-02-21-18-03-05-998832-2_xterm-31141
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [action_client_node-1]: process started with pid [31142]
[action_client_node-1] [INFO] [1677002587.154116513] [quiz_action_client]: Goal ACCEPTED
[action_client_node-1] [INFO] [1677002587.176064572] [quiz_action_client]: Received feedback: 0.16134595831088647
[action_client_node-1] [INFO] [1677002588.176850456] [quiz_action_client]: Received feedback: 0.20720217799030485
[action_client_node-1] [INFO] [1677002589.177748172] [quiz_action_client]: Received feedback: 0.25305839764213073
[action_client_node-1] [INFO] [1677002590.179038345] [quiz_action_client]: Received feedback: 0.3006129957675319
[action_client_node-1] [INFO] [1677002591.180586819] [quiz_action_client]: Received feedback: 0.34646921535474423
[action_client_node-1] [INFO] [1677002592.181749010] [quiz_action_client]: Received feedback: 0.39402381340728865
[action_client_node-1] [INFO] [1677002593.182511389] [quiz_action_client]: Received feedback: 0.4364832759191564
[action_client_node-1] [INFO] [1677002594.183446788] [quiz_action_client]: Received feedback: 0.48064111689401323
[action_client_node-1] [INFO] [1677002595.184787339] [quiz_action_client]: Received feedback: 0.5264973363248816
[action_client_node-1] [INFO] [1677002596.185791724] [quiz_action_client]: Received feedback: 0.5740519342040152
[action_client_node-1] [INFO] [1677002597.186257590] [quiz_action_client]: Received feedback: 0.6199081535372467
[action_client_node-1] [INFO] [1677002598.187589880] [quiz_action_client]: Received feedback: 0.6674627513092767
[action_client_node-1] [INFO] [1677002599.188494210] [quiz_action_client]: Received feedback: 0.713318970533583
[action_client_node-1] [INFO] [1677002600.189721096] [quiz_action_client]: Received feedback: 0.7591751897002865
[action_client_node-1] [INFO] [1677002601.190256153] [quiz_action_client]: Received feedback: 0.8067297872908942
[action_client_node-1] [INFO] [1677002602.191653249] [quiz_action_client]: Received feedback: 0.8542843848135924
[action_client_node-1] [INFO] [1677002603.192633918] [quiz_action_client]: Received feedback: 0.901838982265343
[action_client_node-1] [INFO] [1677002604.193561027] [quiz_action_client]: Received feedback: 0.9476952011666434
[action_client_node-1] [INFO] [1677002605.194995825] [quiz_action_client]: Received feedback: 0.9935514199964307
[action_client_node-1] [INFO] [1677002606.196184360] [quiz_action_client]: Received feedback: 1.0411060172229492
[action_client_node-1] [INFO] [1677002607.199435156] [quiz_action_client]: Status = True
[action_client_node-1] [INFO] [1677002607.200229436] [quiz_action_client]: Total distance travelled: 1.0411060172229492
[INFO] [action_client_node-1]: process has finished cleanly [pid 31142]
user:~/ros2_ws$ ros2 launch actions_quiz actions_quiz_client.launch.py
[INFO] [launch]: All log files can be found below /home/user/.ros/log/2023-02-21-18-03-34-561155-2_xterm-31203
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [action_client_node-1]: process started with pid [31204]
[action_client_node-1] [INFO] [1677002615.493100812] [quiz_action_client]: Goal ACCEPTED
[action_client_node-1] [INFO] [1677002615.494909723] [quiz_action_client]: Received feedback: 0.3787384038792806
[action_client_node-1] [INFO] [1677002616.464175655] [quiz_action_client]: Received feedback: 0.4245946230380404
[action_client_node-1] [INFO] [1677002617.465475579] [quiz_action_client]: Received feedback: 0.4721492206412027
[action_client_node-1] [INFO] [1677002618.470535115] [quiz_action_client]: Received feedback: 0.5180054397143039
[action_client_node-1] [INFO] [1677002619.467903361] [quiz_action_client]: Received feedback: 0.5638616587412163
[action_client_node-1] [INFO] [1677002620.468752612] [quiz_action_client]: Received feedback: 0.6114162561989073
[action_client_node-1] [INFO] [1677002621.469550115] [quiz_action_client]: Received feedback: 0.6589708536009549
[action_client_node-1] [INFO] [1677002622.470299927] [quiz_action_client]: Received feedback: 0.7048270724688152
[action_client_node-1] [INFO] [1677002623.472066998] [quiz_action_client]: Received feedback: 0.7489849128059709
[action_client_node-1] [INFO] [1677002624.472788447] [quiz_action_client]: Received feedback: 0.7965395100299318
[action_client_node-1] [INFO] [1677002625.474142506] [quiz_action_client]: Received feedback: 0.8423957287177358
[action_client_node-1] [INFO] [1677002626.476043942] [quiz_action_client]: Received feedback: 0.8882519473400683
[action_client_node-1] [INFO] [1677002627.477178193] [quiz_action_client]: Received feedback: 0.9324097874304824
[action_client_node-1] [INFO] [1677002628.478165936] [quiz_action_client]: Received feedback: 0.9816627628383543
[action_client_node-1] [INFO] [1677002629.479550538] [quiz_action_client]: Received feedback: 1.0258206027869674
[action_client_node-1] [INFO] [1677002630.480355628] [quiz_action_client]: Received feedback: 1.0733751995754253
[action_client_node-1] [INFO] [1677002631.481817029] [quiz_action_client]: Received feedback: 1.119231417826513
[action_client_node-1] [INFO] [1677002632.482291554] [quiz_action_client]: Received feedback: 1.1650876359956637
[action_client_node-1] [INFO] [1677002633.483679122] [quiz_action_client]: Received feedback: 1.21094385408015
[action_client_node-1] [INFO] [1677002634.484414313] [quiz_action_client]: Received feedback: 1.2584984505198697
[action_client_node-1] [INFO] [1677002635.488305515] [quiz_action_client]: Status = True
[action_client_node-1] [INFO] [1677002635.489127930] [quiz_action_client]: Total distance travelled: 1.2584984505198697
[INFO] [action_client_node-1]: process has finished cleanly [pid 31204]
user:~/ros2_ws$ ros2 launch actions_quiz actions_quiz_client.launch.py
[INFO] [launch]: All log files can be found below /home/user/.ros/log/2023-02-21-18-03-59-783704-2_xterm-31256
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [action_client_node-1]: process started with pid [31257]
[action_client_node-1] [INFO] [1677002641.217640438] [quiz_action_client]: Goal ACCEPTED
[action_client_node-1] [INFO] [1677002641.219981665] [quiz_action_client]: Received feedback: 0.2581535286091521
[action_client_node-1] [INFO] [1677002642.182049597] [quiz_action_client]: Received feedback: 0.3040097474178605
[action_client_node-1] [INFO] [1677002643.178379437] [quiz_action_client]: Received feedback: 0.3498659661927863
[action_client_node-1] [INFO] [1677002644.179788188] [quiz_action_client]: Received feedback: 0.39572218493120564
[action_client_node-1] [INFO] [1677002645.181256553] [quiz_action_client]: Received feedback: 0.44157840363038436
[action_client_node-1] [INFO] [1677002646.181939609] [quiz_action_client]: Received feedback: 0.4874346222875955
[action_client_node-1] [INFO] [1677002647.182945804] [quiz_action_client]: Received feedback: 0.5349892193663441
[action_client_node-1] [INFO] [1677002648.184015478] [quiz_action_client]: Received feedback: 0.5808454379296206
[action_client_node-1] [INFO] [1677002649.184919250] [quiz_action_client]: Received feedback: 0.626701656442639
[action_client_node-1] [INFO] [1677002650.186274234] [quiz_action_client]: Received feedback: 0.6725578749026675
[action_client_node-1] [INFO] [1677002651.187694144] [quiz_action_client]: Received feedback: 0.7184140933069779
[action_client_node-1] [INFO] [1677002652.188959023] [quiz_action_client]: Received feedback: 0.7642703116528381
[action_client_node-1] [INFO] [1677002653.189904019] [quiz_action_client]: Received feedback: 0.808428151483653
[action_client_node-1] [INFO] [1677002654.191730796] [quiz_action_client]: Received feedback: 0.8542843697068397
[action_client_node-1] [INFO] [1677002655.193042505] [quiz_action_client]: Received feedback: 0.8984422094144582
[action_client_node-1] [INFO] [1677002656.194264474] [quiz_action_client]: Received feedback: 0.9459968059508598
[action_client_node-1] [INFO] [1677002657.194889052] [quiz_action_client]: Received feedback: 0.9918530239662318
[action_client_node-1] [INFO] [1677002658.196422578] [quiz_action_client]: Received feedback: 1.0377092419068699
[action_client_node-1] [INFO] [1677002659.197813068] [quiz_action_client]: Received feedback: 1.0835654597700421
[action_client_node-1] [INFO] [1677002660.198793840] [quiz_action_client]: Received feedback: 1.1294216775530124
[action_client_node-1] [INFO] [1677002661.201553534] [quiz_action_client]: Status = True
[action_client_node-1] [INFO] [1677002661.202338137] [quiz_action_client]: Total distance travelled: 1.1294216775530124
[INFO] [action_client_node-1]: process has finished cleanly [pid 31257]
user:~/ros2_ws$ ros2 launch actions_quiz actions_quiz_client.launch.py
[INFO] [launch]: All log files can be found below /home/user/.ros/log/2023-02-21-18-04-48-313403-2_xterm-31341
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [action_client_node-1]: process started with pid [31350]
[action_client_node-1] [INFO] [1677002689.231441563] [quiz_action_client]: Goal ACCEPTED
[action_client_node-1] [INFO] [1677002689.233447394] [quiz_action_client]: Received feedback: 1.2856724814883693
[action_client_node-1] [INFO] [1677002690.204366492] [quiz_action_client]: Received feedback: 1.3315286984666206
[action_client_node-1] [INFO] [1677002691.205984090] [quiz_action_client]: Received feedback: 1.3773849153494717
[action_client_node-1] [INFO] [1677002692.206846053] [quiz_action_client]: Received feedback: 1.424939510531698
[action_client_node-1] [INFO] [1677002693.208646095] [quiz_action_client]: Received feedback: 1.4707957272117538
[action_client_node-1] [INFO] [1677002694.209811662] [quiz_action_client]: Received feedback: 1.5149535653982693
[action_client_node-1] [INFO] [1677002695.210380787] [quiz_action_client]: Received feedback: 1.5608097818721618
[action_client_node-1] [INFO] [1677002696.211734965] [quiz_action_client]: Received feedback: 1.606665998236987
[action_client_node-1] [INFO] [1677002697.213043215] [quiz_action_client]: Received feedback: 1.6542205928675546
[action_client_node-1] [INFO] [1677002698.214329242] [quiz_action_client]: Received feedback: 1.7000768090017242
[action_client_node-1] [INFO] [1677002699.215630787] [quiz_action_client]: Received feedback: 1.7459330250185112
[action_client_node-1] [INFO] [1677002700.216308293] [quiz_action_client]: Received feedback: 1.7900908625508067
[action_client_node-1] [INFO] [1677002701.217561925] [quiz_action_client]: Received feedback: 1.8359470783292204
[action_client_node-1] [INFO] [1677002702.218559051] [quiz_action_client]: Received feedback: 1.8818032939821439
[action_client_node-1] [INFO] [1677002703.219958146] [quiz_action_client]: Received feedback: 1.9276595095068412
[action_client_node-1] [INFO] [1677002704.220412529] [quiz_action_client]: Received feedback: 1.9735157249005817
[action_client_node-1] [INFO] [1677002705.220991581] [quiz_action_client]: Received feedback: 2.0193719401606267
[action_client_node-1] [INFO] [1677002706.222320829] [quiz_action_client]: Received feedback: 2.0669265336194957
[action_client_node-1] [INFO] [1677002707.223705721] [quiz_action_client]: Received feedback: 2.1144811269285886
[action_client_node-1] [INFO] [1677002708.225153640] [quiz_action_client]: Received feedback: 2.160337341760508
[action_client_node-1] [INFO] [1677002709.228831496] [quiz_action_client]: Status = True
[action_client_node-1] [INFO] [1677002709.229721483] [quiz_action_client]: Total distance travelled: 2.160337341760508
[INFO] [action_client_node-1]: process has finished cleanly [pid 31350]
user:~/ros2_ws$ ros2 launch actions_quiz actions_quiz_client.launch.py
[INFO] [launch]: All log files can be found below /home/user/.ros/log/2023-02-21-18-05-14-470140-2_xterm-31402
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [action_client_node-1]: process started with pid [31403]
[action_client_node-1] [INFO] [1677002715.160711416] [quiz_action_client]: Goal ACCEPTED
[action_client_node-1] [INFO] [1677002715.162940713] [quiz_action_client]: Received feedback: 0.22758271009582112
[action_client_node-1] [INFO] [1677002716.132939574] [quiz_action_client]: Received feedback: 0.27004217086168303
[action_client_node-1] [INFO] [1677002717.134352696] [quiz_action_client]: Received feedback: 0.31080325317034774
[action_client_node-1] [INFO] [1677002718.137797898] [quiz_action_client]: Received feedback: 0.3549610923064474
[action_client_node-1] [INFO] [1677002719.139108805] [quiz_action_client]: Received feedback: 0.39572217455467984
[action_client_node-1] [INFO] [1677002720.140468863] [quiz_action_client]: Received feedback: 0.4364832567709398
[action_client_node-1] [INFO] [1677002721.141694168] [quiz_action_client]: Received feedback: 0.46705406841100167
[action_client_node-1] [INFO] [1677002722.142283618] [quiz_action_client]: Received feedback: 0.5078151505666929
[action_client_node-1] [INFO] [1677002723.143279072] [quiz_action_client]: Received feedback: 0.5366875837378801
[action_client_node-1] [INFO] [1677002724.144543239] [quiz_action_client]: Received feedback: 0.5706551521497171
[action_client_node-1] [INFO] [1677002725.146018270] [quiz_action_client]: Received feedback: 0.6063210989520631
[action_client_node-1] [INFO] [1677002726.146473142] [quiz_action_client]: Received feedback: 0.6368919104714466
[action_client_node-1] [INFO] [1677002727.147407304] [quiz_action_client]: Received feedback: 0.6640659651349033
[action_client_node-1] [INFO] [1677002728.149873164] [quiz_action_client]: Received feedback: 0.69463677660756
[action_client_node-1] [INFO] [1677002729.149346290] [quiz_action_client]: Received feedback: 0.7286043448800867
[action_client_node-1] [INFO] [1677002730.151833321] [quiz_action_client]: Received feedback: 0.764270291530597
[action_client_node-1] [INFO] [1677002731.152095742] [quiz_action_client]: Received feedback: 0.7999362381433203
[action_client_node-1] [INFO] [1677002732.153311888] [quiz_action_client]: Received feedback: 0.8373005631242496
[action_client_node-1] [INFO] [1677002733.154564571] [quiz_action_client]: Received feedback: 0.8780616448709829
[action_client_node-1] [INFO] [1677002734.156058392] [quiz_action_client]: Received feedback: 0.9103308345484348
[action_client_node-1] [INFO] [1677002735.159688434] [quiz_action_client]: Status = True
[action_client_node-1] [INFO] [1677002735.160481379] [quiz_action_client]: Total distance travelled: 0.9103308345484348
[INFO] [action_client_node-1]: process has finished cleanly [pid 31403]
user:~/ros2_ws$ ros2 launch actions_quiz actions_quiz_client.launch.py
[INFO] [launch]: All log files can be found below /home/user/.ros/log/2023-02-21-18-05-39-190215-2_xterm-31455
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [action_client_node-1]: process started with pid [31456]
[action_client_node-1] [INFO] [1677002740.301115911] [quiz_action_client]: Goal ACCEPTED
[action_client_node-1] [INFO] [1677002740.304144706] [quiz_action_client]: Received feedback: 0.18682162628981064
[action_client_node-1] [INFO] [1677002741.253888776] [quiz_action_client]: Received feedback: 0.21909081624274646
[action_client_node-1] [INFO] [1677002742.260830726] [quiz_action_client]: Received feedback: 0.24966162776328302
[action_client_node-1] [INFO] [1677002743.255611990] [quiz_action_client]: Received feedback: 0.28193081768610284
[action_client_node-1] [INFO] [1677002744.256614150] [quiz_action_client]: Received feedback: 0.3192951428387547
[action_client_node-1] [INFO] [1677002745.258661061] [quiz_action_client]: Received feedback: 0.3583578463816396
[action_client_node-1] [INFO] [1677002746.259873609] [quiz_action_client]: Received feedback: 0.39572217148340216
[action_client_node-1] [INFO] [1677002747.261446336] [quiz_action_client]: Received feedback: 0.4347848749698444
[action_client_node-1] [INFO] [1677002748.262436228] [quiz_action_client]: Received feedback: 0.4755459568353011
[action_client_node-1] [INFO] [1677002749.263786351] [quiz_action_client]: Received feedback: 0.5163070386646943
[action_client_node-1] [INFO] [1677002750.265166808] [quiz_action_client]: Received feedback: 0.555369742048913
[action_client_node-1] [INFO] [1677002751.266348112] [quiz_action_client]: Received feedback: 0.5978292022076175
[action_client_node-1] [INFO] [1677002752.267080742] [quiz_action_client]: Received feedback: 0.6351935271098106
[action_client_node-1] [INFO] [1677002753.268603819] [quiz_action_client]: Received feedback: 0.6759546087794711
[action_client_node-1] [INFO] [1677002754.272382447] [quiz_action_client]: Received feedback: 0.7150173120035483
[action_client_node-1] [INFO] [1677002755.273869484] [quiz_action_client]: Received feedback: 0.7574767719803194
[action_client_node-1] [INFO] [1677002756.274355248] [quiz_action_client]: Received feedback: 0.8016346102993432
[action_client_node-1] [INFO] [1677002757.275319396] [quiz_action_client]: Received feedback: 0.8440940701645748
[action_client_node-1] [INFO] [1677002758.278858947] [quiz_action_client]: Received feedback: 0.8848551515806666
[action_client_node-1] [INFO] [1677002759.278249779] [quiz_action_client]: Received feedback: 0.9290129897188939
[action_client_node-1] [INFO] [1677002760.282547336] [quiz_action_client]: Status = True
[action_client_node-1] [INFO] [1677002760.283331621] [quiz_action_client]: Total distance travelled: 0.9290129897188939
[INFO] [action_client_node-1]: process has finished cleanly [pid 31456]
By looking at the feedback messages from the client node, it seems that the measurements do not
always start from 0. I am not sure why.
Thank you @girishkumar.kannan. I made the modifications as requested, however, I still had the same problem. But, thanks to your suggestions, I was able to find the problem. The measurements for the starting positions were being taken before I was sending the goal, which made the computed distance larger. So I added another Boolean that sets to True when the execute_callback method is called, which will then take the initial measurements. After that, I was still having a problem with the initial feedback measurement. I was getting a large value for the first feedback message, then the rest of the messages go back to normal. I fixed this issue by resetting to 0 the variables that store the coordinates from the initial and current measurements before the for loop.
import time
import rclpy
import numpy as np
from rclpy.action import ActionServer
from rclpy.node import Node
from rclpy.qos import ReliabilityPolicy, QoSProfile
from actions_quiz_msg.action import Distance
from nav_msgs.msg import Odometry
from std_msgs.msg import Float64
from geometry_msgs.msg import Point
# Import the libraries to use executors and callback groups
from rclpy.callback_groups import ReentrantCallbackGroup, MutuallyExclusiveCallbackGroup
from rclpy.executors import MultiThreadedExecutor, SingleThreadedExecutor
class QuizActionServer(Node):
def __init__(self):
super().__init__('quiz_action_server')
self.group1 = MutuallyExclusiveCallbackGroup()
self.group2 = MutuallyExclusiveCallbackGroup()
self._action_server = ActionServer(
self, Distance, 'distance_as', self.execute_callback, callback_group=self.group1)
self.subscriber_ = self.create_subscription(
Odometry, '/odom', self.odometry_callback,
QoSProfile(depth=10, reliability=ReliabilityPolicy.RELIABLE), callback_group=self.group2)
self.publisher_ = self.create_publisher(Float64, '/total_distance', 10)
self.stored_init_meas = False
# publisher variable used to send message to the /total_distance topic
self.pub_var = Float64()
# used to store initial measurement
self.xy_0 = Point()
# used to store current measurement
self.xy_curr = Point()
# used to store the distance travelled
self.curr_dist = 0.0
# bool to be set to True when the execute_callback method is called
self.execute_goal = False
def odometry_callback(self, msg):
# storing the initial measurement before setting the goal
if self.stored_init_meas is False and self.execute_goal is True:
self.xy_0 = msg.pose.pose.position
self.get_logger().warn(f'Initial measurement: {self.xy_0}')
# needs to be set back to False at the end of the program
self.stored_init_meas = True
# storing current measurement
self.xy_curr = msg.pose.pose.position
# self.get_logger().info('Odometry measurements received...')
def execute_callback(self, goal_handle):
self.get_logger().info('Initiating goal ...')
self.execute_goal = True
feedback_msg = Distance.Feedback()
self.get_logger().warn(
f'curr_dist value before reset: {self.curr_dist}')
self.curr_dist = 0.0
self.get_logger().warn(
f'curr_dist value after reset: {self.curr_dist}')
# resetting coordinates
self.xy_0.x = 0.0
self.xy_0.y = 0.0
self.xy_curr.x = 0.0
self.xy_curr.y = 0.0
for i in range(goal_handle.request.seconds):
# calculating current distance travelled
self.curr_dist = ((self.xy_curr.x-self.xy_0.x) **
2 + (self.xy_curr.y-self.xy_0.y)**2)**0.5
# storing current distance in the feedback part of the action message
feedback_msg.current_dist = self.curr_dist
# publishing current distance to the /total_distance topic
self.pub_var.data = self.curr_dist
self.publisher_.publish(self.pub_var)
# publishing current distance as feedback of the Action server
goal_handle.publish_feedback(feedback_msg)
# waiting for 1 second
time.sleep(1.0)
# updating the goal_handle state to SUCCEED
goal_handle.succeed()
# resetting Boolean variables
self.stored_init_meas = False
self.execute_goal = False
# returning the total distance in the result part of the action message
result = Distance.Result()
result.status = True
result.total_dist = self.curr_dist
self.get_logger().info(
f'The total distance travelled is: {result.total_dist}')
return result
def main(args=None):
rclpy.init(args=args)
my_quiz_server = QuizActionServer()
# Create a MultiThreadedExecutor
executor = MultiThreadedExecutor(num_threads=4)
# adding node to executor
executor.add_node(my_quiz_server)
try:
# spin the executor
executor.spin()
finally:
executor.shutdown()
my_quiz_server.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
I thought of mentioning this step to you earlier, but then I noted that whenever the flag is false in the odometry_callback(), your self.xy_0 would be replaced with new value. So I though this step might be unnecessary.
Anyways, you have solved it yourself - which means you will remember this step for the future.
