#!/usr/bin/env python3
# importing some libraries
"""
.. module: Thirdnode
:platform: unix
:synopsis: Track the number of times of cancelling and reaching goal
.. moduleauthor:: Ehsan Fatemi
This node is to track the number of times a
robot reaches a goal and
the number of times a goal is canceled.
Subscribes to:
/reaching_goal/result
"""
from std_srvs.srv import Empty,EmptyResponse
import assignment_2_2022.msg
import rospy
reaching_times =0
cancelation_times = 0
#this function is called if the service is called
#it prints the current values of "cancelation_times" and "reaching_times" and sends an empty response.
[docs]def cl_bck(req):
"""This function is called when the service
*"reach_cancel_ints"* is called. It prints the
current values of the global variables
*cancelation_times* and *reaching_times* and returns
an empty response.
Args:
req (msg): a request message of type *Empty*.
"""
global cancelation_times,reaching_times
print(f"Canceled Objective's times {cancelation_times} , Reached Objective's times: {reaching_times}")
return EmptyResponse()
[docs]def cl_bck_sub(data):
"""
This function checks the status field of the message to
determine whether the goal was reached or canceled.
If the goal was canceled, it increments the
*cancelation_times* counter. If the goal was reached,
it increments the *reaching_times* counter.
Args:
data : a message of type *assignment_2_2022.msg.PlanningActionResult*
"""
if data.status.status == 2:
global cancelation_times
cancelation_times += 1
elif data.status.status == 3:
global reaching_times
reaching_times += 1
#This code is the main function of the ROS node
#It specifies that when a message is received, the function cl_bck_sub should be called to handle the message
#it calls rospy.spin() which keeps the node running and listening for new messages or service calls
if __name__ == "__main__":
rospy.logwarn("You have started the Code")
rospy.init_node('reach_cancel_node')
rospy.Subscriber("/reaching_goal/result", assignment_2_2022.msg.PlanningActionResult, cl_bck_sub)
rospy.Service('reach_cancel_ints', Empty, cl_bck)
rospy.spin()