cancel navigation goal from pyton
After i send the action request other service callback function is not being called.
#!/usr/bin/env python3
import rospy
from tts_ros.srv import TTS
from cognition.srv import Mission, Perception, PerceptionResponse, MissionResponse
import actionlib
from actionlib_msgs.msg import *
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from tts_ros.srv import *
import math
from threading import Thread, Lock
from geometry_msgs.msg import PoseArray, Pose
from sensor_msgs.msg import Image
import rosservice
# take second element for sort
def sortByDist(elem):
return elem[1]
def speak_text_client(text):
service_list = rosservice.get_service_list()
if 'tts_service' in service_list:
rospy.wait_for_service('tts_service')
try:
speak_text = rospy.ServiceProxy('tts_service', TTS)
resp = speak_text(text)
return resp.result
except rospy.ServiceException as e:
print("Service call failed: %s"%e)
else:
print('tts_service is not available!')
def move_base_client(pose):
client = actionlib.SimpleActionClient('/move_base', MoveBaseAction)
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = "map"
goal.target_pose.header.stamp = rospy.Time.now()
rospy.loginfo('Waiting for move base action server!')
client.wait_for_server()
# Creates a goal to send to the action server.
goal.target_pose.pose = pose
rospy.loginfo('Sending move base goal!')
# Sends the goal to the action server.
client.send_goal(goal)
# Waits for the server to finish performing the action.
client.wait_for_result()
return client.get_result()
class CateringJob:
def __init__(self):
rospy.init_node('catering_task', anonymous=True)
self.BINI_RADI = 5
self.GUEST_RADI = 0.5
self.CURR_GOAL_IND = 0
rospy.Service('bini_cmd', Mission, self.bini_cmd_req_callback)
rospy.Subscriber("poses", Image, self.perception_callback)
self.cancel_goal_pub = rospy.Publisher('/move_base/cancel', GoalID)
self.mission = PoseArray()
self.served_areas = PoseArray()
self.perception_poses = PoseArray()
self.mutex = Lock()
self.main_thread = Thread(target = self.main_thread)
self.main_thread.start()
def __del__(self):
self.main_thread.join()
def main_thread(self):
while not rospy.is_shutdown():
self.mutex.acquire()
if len(self.mission.poses) > 0:
result = move_base_client(self.mission.poses[self.CURR_GOAL_IND])
if result:
speak_text_client('hello i am bini, can i help you')
rospy.sleep(1)
else:
rospy.logerr('Move base goal failed!')
self.CURR_GOAL_IND += 1
if self.CURR_GOAL_IND == len(self.mission.poses):
self.mission.poses.clear()
self.CURR_GOAL_IND = 0
self.mutex.release()
def perception_callback(self, poses):
self.mutex.acquire()
rospy.loginfo('Got perception data!')
if len(poses) > 0:
valid_poses = []
robot_pose = self.perception_poses.poses[0]
for i in range(1, len(self.perception_poses.mission.poses)):
guest_pose = self.perception_poses.poses[i]
bini_person_dist = math.sqrt((robot_pose.x-guest_pose.x)**2+(robot_pose.y-guest_pose.y)**2)
if bini_person_dist <= self.BINI_RADI:
for served_area in self.served_areas.poses:
already_served = True if math.sqrt((served_area.x-guest_pose.x)**2+(served_area.y-guest_pose.y)**2) < self.GUEST_RADI else False
if not already_served:
valid_poses.append([guest_pose, bini_person_dist])
# TODO : mission should be updated according
if len(valid_poses[0].poses) > 0:
valid_poses.sort(key=sortByDist)
self.mission.poses[self.CURR_GOAL_IND:self.CURR_GOAL_IND] = valid_poses[0].poses
rospy.loginfo('Mission updated!')
self.served_areas.poses.extend(valid_poses[0].poses)
valid_poses.clear()
self.perception_poses.poses.clear()
self.mutex.release()
return PerceptionResponse(1)
def bini_cmd_req_callback(self, req):
self.mutex.acquire()
rospy.loginfo('Got mission cmd request')
if len(req.mission.poses) > 0:
self.mission.poses.extend(req.mission.poses)
else:
self.mission.poses.clear()
self.cancel_goal_pub.publish({})
self.mutex.release()
return MissionResponse(1)
if __name__ == '__main__':
catring_job = CateringJob()
rospy.spin()
Asked by dinesh on 2021-09-25 13:43:02 UTC
Comments
Could you please clarify what you expect from us? Your code 'sample' is quite extensive, and there isn't much in the way of explanation.
What is your question?
Asked by gvdhoorn on 2021-09-26 07:06:21 UTC
@dinesh You need to check how much CPU is used by this ros node. If it's 100%, which I suspect it is, the node is not going to perform well. When the node is not doing work, the CPU use should be very small.
Also, I don't remember the details, but there is something unusual about client.get_result() for simple action client and move_base: possibly the issue is that it returns None? You should add a print() call to check what you are getting back. For move_base, I think that client.get_state() can be used to check whether goal was reached or not.
Asked by Mike Scheutzow on 2021-09-26 09:00:01 UTC
@gvdhoorn the main issue what that after i send the move base goal command this action request was blocking other functions like service and subscriber callback.
Asked by dinesh on 2021-09-26 10:52:23 UTC