How to cancel all goals in my own action server rightly?

asked 2022-01-24 05:53:04 -0500

Rikyuugo gravatar image

Hi,I'm whriting an action to realize multi target navigation and I want to cancel all goals after sending instruction.However,whether I send actiontopic/cancel an empty message or use client.cancelAllGoals() ,it's all useless.How to do it rightly? This is my partial code,is it because I called move_base_client?

    void TranslateJson(const string strData,Server *res,tasks_resolution::taskactionFeedback &feedback,tasks_resolution::taskactionResult &result){
    MoveBaseClient ac("move_base",true);
   for(int k = 0 ;k<nPatrolAction;k++){
            if (taskdata.TaskData[i].ActionGroup[j].PatrolAction[k].ActionTypeId==1)
            {
                actiontypeid = START;
            }
            if (taskdata.TaskData[i].ActionGroup[j].PatrolAction[k].ActionTypeId==2)
            {
                actiontypeid = MOVING;
            }
            if (taskdata.TaskData[i].ActionGroup[j].PatrolAction[k].ActionTypeId==3)
            {
                actiontypeid = WAITING;
            }
            if (taskdata.TaskData[i].ActionGroup[j].PatrolAction[k].ActionTypeId==4)
            {
                actiontypeid = RFID_OPEN;
            }
            if (taskdata.TaskData[i].ActionGroup[j].PatrolAction[k].ActionTypeId==5)
            {
                actiontypeid = RFID_CLOSE;
            }


            switch (actiontypeid)
            {
            case START:
                break;
            case MOVING:
                {for(int m = 0;m<narraypoint;m++){
                    if(this_floor_navigation_goals.pointinfoArray[m].NavigationPointId == taskdata.TaskData[i].ActionGroup[j].PatrolAction[k].NavigationPointId){
                        while(!ac.waitForServer(ros::Duration(5.0))){
                            ROS_INFO("Waiting for the move_base action server to come up");
                        }
                        move_base_msgs::MoveBaseGoal goal;
                        goal.target_pose.header.frame_id = "map";
                        goal.target_pose.header.stamp = ros::Time::now();
                        goal.target_pose.pose.position.x = this_floor_navigation_goals.pointinfoArray[m].Pose.position.x;
                        goal.target_pose.pose.position.y = this_floor_navigation_goals.pointinfoArray[m].Pose.position.y;
                        goal.target_pose.pose.position.z = this_floor_navigation_goals.pointinfoArray[m].Pose.position.x;

                        goal.target_pose.pose.orientation.x = this_floor_navigation_goals.pointinfoArray[m].Pose.orientation.x;
                        goal.target_pose.pose.orientation.y = this_floor_navigation_goals.pointinfoArray[m].Pose.orientation.y;
                        goal.target_pose.pose.orientation.z = this_floor_navigation_goals.pointinfoArray[m].Pose.orientation.z;
                        goal.target_pose.pose.orientation.w = this_floor_navigation_goals.pointinfoArray[m].Pose.orientation.w;
                        ac.sendGoal(goal);
                        ROS_INFO("the car is moving");
                        feedback.feedback = ("Now is going to %s",this_floor_navigation_goals.pointinfoArray[m].NavigatePointName);
                        res->publishFeedback(feedback);
                        ac.waitForResult();
                        if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED){
                            feedback.feedback = "the car has arrived rule point";
                            res->publishFeedback(feedback);
                        }else{
                            feedback.feedback = "the car can not reach the goal";
                            res->publishFeedback(feedback);
                        }

                    }
            }
            tasks_resolution::othermsgs othermsgs;
            othermsgs.ActionTypeId = taskdata.TaskData[i].ActionGroup[j].PatrolAction[k].ActionTypeId;
            othermsgs.WaitingMilliseconds = taskdata.TaskData[i].ActionGroup[j].PatrolAction[k].WaitingMilliseconds;
            pub_other_msgs.publish(othermsgs);}
                break;
            case WAITING:{
                int sleep_time = taskdata.TaskData[i].ActionGroup[j].PatrolAction[k].WaitingMilliseconds/1000;
                cout<<"Sleep TIME:"<<sleep_time<<endl;
                feedback.feedback = ("Waiting Time:%d s",sleep_time);
                res->publishFeedback(feedback);
                ros::Duration(sleep_time).sleep();
                result.result = "Finish Sleeped.";
                res->setSucceeded(result);
            }
            case RFID_OPEN:
                break;    
            case RFID_CLOSE:
                break;

            default:
                ROS_INFO("Default!");
                break;
            }

        }
}
    void execute(const tasks_resolution::taskactionGoalConstPtr& demo_goal,Server* as){
        ros::Rate r(2);
        string goal = demo_goal->goal;
        tasks_resolution::taskactionFeedback feedback;
        tasks_resolution::taskactionResult result;
        TranslateJson(goal,as,feedback,result);
    }
    int main(int argc,char **argv){
        ros::init(argc,argv,"dodishes");
        ros::NodeHandle nh;
        Server server(nh,"do_dishes",boost::bind(&execute,_1,&server),false);
        server.start();
        ros::spin();
        return 0 ;
    }
edit retag flag offensive close merge delete