Knowledge base update to remove one goal after failed action

[cat2510]

Hi, I am trying to write a C++ code to remove a goal representing a pick and place task when the action pick has failed. The robot should then skip the succeeding place action, but continue performing the later tasks in the original plan. I am not sure how this should be done properly, since I have the impression that the planner does not get updated with the changed set of goals.

RPPickInterface::RPPickInterface(ros::NodeHandle` &nh) : _nh(nh) {
    _pickclient = _nh.serviceClient<std_srvs::Trigger>("/pick_gui");
    _pickclient.waitForExistence();
}


bool RPPickInterface::concreteCallback(
        const rosplan_dispatch_msgs::ActionDispatch_<std::allocator<void>>::ConstPtr &msg) {

    std_srvs::Trigger srv;
    std::cout << _pickclient.call(srv) << std::endl;
    int response;
    if (srv.response.success){
        response = 1;
    }
    else{response = 0;
    RPPickInterface::update_after_fail_to_pick();}
    ROS_INFO_STREAM("PICK_INTERFACE: Action completed with success = " << response << " and message " <<
                    srv.response.message);
    return srv.response.success;
}

void RPPickInterface::update_after_fail_to_pick(){
    // update knowledge base
    rosplan_knowledge_msgs::KnowledgeUpdateServiceArray updatePredSrv;

    // remove next goal i.e. (place) action
    rosplan_knowledge_msgs::KnowledgeItem item3;
    item3.knowledge_type = rosplan_knowledge_msgs::KnowledgeItem::FACT;
    item3.attribute_name = "box_at_wp";
    item3.values.clear();
    diagnostic_msgs::KeyValue boxpair;
    boxpair.key = "box";
    boxpair.value = "green_box";
    item3.values.push_back(boxpair);
    diagnostic_msgs::KeyValue pair2;
    pair2.key = "wp";
    pair2.value = "wp5";
    item3.values.push_back(pair2);
    updatePredSrv.request.knowledge.push_back(item3);
    updatePredSrv.request.update_type.push_back(rosplan_knowledge_msgs::KnowledgeUpdateService::Request::REMOVE_GOAL);

    bool res = update_knowledge_client.call(updatePredSrv);
    ROS_INFO_STREAM("Update after failure result: "<< res);
    }