Robotics StackExchange | Archived questions

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

Answers