KCL: (RPActionInterface) could not call Knowledge Base for operator details, goto_waypoint

[karolinajudzentyte]

I have created my own action (very similar to rpmovebase) file. When trying to launch am getting:

[ INFO] [1648048101.336796716]: KCL: (/rosplan_knowledge_base) Ready to receive [ INFO] [1648048101.340591621]: KCL: (/rosplan_planner_interface) Ready to receive [ERROR] [1648048101.347032282]: KCL: (RPActionInterface) could not call Knowledge Base for operator details, goto_waypoint

Have no idea of how to go around to even debugging this. Any help would be appreciated

Launch file

domain: (define (domain spot_demo)

(:requirements :strips :typing :fluents :disjunctive-preconditions :durative-actions)

(:types waypoint robot )

(:predicates (robot_at ?v - robot ?wp - waypoint) (connected ?from ?to - waypoint) (visited ?wp - waypoint) )

(:functions (distance ?wp1 ?wp2 - waypoint) )

;; Move to any waypoint, avoiding terrain (:durative-action goto_waypoint :parameters (?v - robot ?from ?to - waypoint) :duration ( = ?duration 60) :condition (and (at start (robot_at ?v ?from)) :effect (and (at end (visited ?to)) (at end (robot_at ?v ?to)) (at start (not (robot_at ?v ?from)))) )

[gerardcanal]

Hello, I can't see anything obviously wrong there. Can you try to see if the knowledge base is responsive by running rosservice call /rosplan_knowledge_base/domain/operator_details "name: 'goto_waypoint'"? I believe this is the call failing, so we should see why is the call failing (that output is in a try/catch).

[karolinajudzentyte]

Hello,

Thank you for such a quick reply. By running: rosservice call /rosplan_knowledge_base/domain/operator_details "name: 'goto_waypoint'" -> ERROR: service [/rosplan_knowledge_base/domain/operator_details] responded with an error: while running: rosservice call /rosplan_knowledge_base/domain/operators -> operators: []

I can see that the goto_waypoint is not being added to the knowledge base. However, I don't know why.

Adding my header (same as used in move base)

include <ros/ros.h>

include

include

include

include <actionlib/client/simple_action_client.h>

include <navigate_2d_spot/NavigateToGoalAction.h>

include <geometry_msgs/PoseStamped.h>

include <std_srvs/Empty.h>

include <tf2/LinearMath/Quaternion.h>

include <rosplan_action_interface/RPActionInterface.h>

ifndef KCL_movebase

define KCL_movebase

namespace KCL_rosplan {

class RPMoveBase: public RPActionInterface
{

  public:
    RPMoveBase(std::string &actionserver);
    bool wpIDtoPoseStamped(std::string wpID, geometry_msgs::PoseStamped &result);
    bool concreteCallback(const rosplan_dispatch_msgs::ActionDispatch::ConstPtr& msg);

  private:

    actionlib::SimpleActionClient<navigate_2d_spot::NavigateToGoalAction> action_client_;
    ros::NodeHandle nh_;

    std::string waypoint_frameid_;
    std::string wp_namespace_;
};

}

endif

And my action server for ROSplan. Again very similar to move_bese (rpmovebase)

include "include/RPNavigate.h"

namespace KCL_rosplan {

// constructor
RPNavigate::RPNavigate(std::string &actionserver) : action_client_(actionserver, true) {

    // create a node handle to manage communication with ROS network
    ros::NodeHandle nh("~");

    // get waypoints reference frame from param server
    nh.param<std::string>("waypoint_frameid", waypoint_frameid_, "map");
    nh.param<std::string>("wp_namespace", wp_namespace_, "/rosplan_demo_waypoints/wp");

    // setup a move base clear costmap client (to be able to send clear costmap requests later on)
    // clear_costmaps_client_ = nh.serviceClient<std_srvs::Empty>("/move_base/clear_costmaps");
    ROS_INFO("Constructor");
}

bool RPNavigate::wpIDtoPoseStamped(std::string wpID, geometry_msgs::PoseStamped &result) {
    ROS_INFO("Method");
    ros::NodeHandle nh;
    std::vector<double> wp;
    if(nh.hasParam(wp_namespace_ + "/" + wpID)) {
        if(nh.getParam(wp_namespace_ + "/" + wpID, wp)) {
            if(wp.size() == 3) {
                result.header.frame_id = waypoint_frameid_;
                result.pose.position.x = wp[0];
                result.pose.position.y = wp[1];

                tf2::Quaternion q;
                q.setRPY(0.0, 0.0, wp[2]);
                result.pose.orientation.x = q[0];
                result.pose.orientation.y = q[1];
                result.pose.orientation.z = q[2];
                result.pose.orientation.w = q[3];

                return true;
            }
            else {
                ROS_ERROR("wp size must be equal to 3 : (x, y, and theta)");
                return false;
            }
        }
    }
    else
        return false;
}

// action dispatch callback
bool RPNavigate::concreteCallback(const rosplan_dispatch_msgs::ActionDispatch::ConstPtr& msg) {
    ROS_INFO("Callback");

    // get waypoint ID from action dispatch msg
    std::string wpID;
    bool found = false;
    // iterating over parameters (e.g. kenny, wp0, wp1)
    for(size_t i = 0; i < msg->parameters.size(); i++) {
        // check their keys
        if(0 == msg->parameters[i].key.compare("to") or 0 == msg->parameters[i].key.compare("w1")) {
            // wp id found in msg params
            wpID = msg->parameters[i].value;
            found = true;
        }
    }
    if(!found) {
        ROS_INFO("KCL: (%s) aborting action dispatch; PDDL action missing required parameter ?to", params.name.c_str());
        return false;
    }

    // get waypoint coordinates from its ID via query to parameter server
    geometry_msgs::PoseStamped pose;
    if(!wpIDtoPoseStamped(wpID, pose)) {
        ROS_ERROR("Waypoint not found in parameter server");
        return false;
    }

    ROS_INFO("KCL: (%s) waiting for navigate action server to start", params.name.c_str());
    action_client_.waitForServer();

    navigation_2d_spot::NavigateToGoalGoal final_goal;

    geometry_msgs::PoseStamped final_pose; 

    final_goal.target_pose = pose;
    final_goal.duration = 3.0;
    final_goal.precise_positioning = true;
    action_client_.sendGoal(final_goal);

    bool finished_before_timeout = action_client_.waitForResult();
    if (finished_before_timeout) {

        actionlib::SimpleClientGoalState state = action_client_.getState();
        ROS_INFO("KCL: (%s) action finished: %s", params.name.c_str(), state.toString().c_str());

        if(state == actionlib::SimpleClientGoalState::SUCCEEDED) {

            // publish feedback (achieved)
            return true;

        } else {
            action_client_.cancelAllGoals();
            ROS_INFO("KCL: (%s) action timed out", params.name.c_str());
            return false;
        }
    } else {
        // timed out (failed)
        action_client_.cancelAllGoals();
        ROS_INFO("KCL: (%s) action timed out", params.name.c_str());
        return false;
    }
}

} // close namespace

int main(int argc, char **argv) {

ros::init(argc, argv, "rosplan_interface_navigate");

ros::NodeHandle nh("~");

std::string actionserver;
nh.param("action_server", actionserver, std::string("/navigate_to_goal_spot"));  // Python action node for navigation

KCL_rosplan::RPNavigate rpmb(actionserver);

rpmb.runActionInterface();

return 0;

}

[gerardcanal]

Hi, I believe this is not related to the Action interface but in the loading of the domain in the KB. There's probably an issue in the domain and the KB can't load it. Can you check if there are any errors when the Knowledge Base launches?

[gerardcanal]

Or actually, can you check if the domain is present in the path you specify?

<arg name="domain_path" value="$(find navigation_2d_spot)/common/no_move_base/domain_spot.pddl" />

You load the domain from there, so the file should be inside the navigation_2d_spot ros package, common/no_move_base/domain_spot.pddl. make sure the file is there.

You can check it by running the following in the terminal: cat $(rospack find navigation_2d_spot)/common/no_move_base/domain_spot.pddl Does this print your domain?

[karolinajudzentyte]

Hello again,

Yes it does print: cat $(rospack find navigation_2d_spot)/common/no_move_base/domain_spot.pddl (define (domain spot_demo)

(:requirements :strips :typing :fluents :disjunctive-preconditions :durative-actions)

(:types waypoint robot )

(:predicates (robot_at ?v - robot ?wp - waypoint) (connected ?from ?to - waypoint) (visited ?wp - waypoint) )

(:functions (distance ?wp1 ?wp2 - waypoint) )

;; Move to any waypoint, avoiding terrain (:action goto_waypoint :parameters (?v - robot ?from ?to - waypoint) :condition (and (at start (robot_at ?v ?from)) :effect (and (at end (visited ?to)) (at end (robot_at ?v ?to)) (at start (not (robot_at ?v ?from)))) )

)

and I can see with other service calls that predicates and functions are there

rosservice call /rosplan_knowledge_base/domain/functions items:

• name: "distance" typed_parameters:

  • key: "wp1" value: "waypoint"

  • key: "wp2" value: "waypoint"

[gerardcanal]

Hi again, Your domain's parentheses are not right, that is why it doesn't load the actions properly. The parser for some reason can read it though.

You are missing a closing parenthesis at the end to close the domain, and another one after the at start condition to close the condition block (there should be 3 instead of 2).

This domain works:

(define (domain spot_demo)

(:requirements :strips :typing :fluents :disjunctive-preconditions :durative-actions)

(:types
waypoint
robot
)

(:predicates
(robot_at ?v - robot ?wp - waypoint)
(connected ?from ?to - waypoint)
(visited ?wp - waypoint)
)

(:functions
(distance ?wp1 ?wp2 - waypoint)
)

;; Move to any waypoint, avoiding terrain
(:durative-action goto_waypoint
:parameters (?v - robot ?from ?to - waypoint)
:duration ( = ?duration 60)
:condition (and
(at start (robot_at ?v ?from)))
:effect (and
(at end (visited ?to))
(at end (robot_at ?v ?to))
(at start (not (robot_at ?v ?from))))
)
)

[karolinajudzentyte]

Thanks for your response. I did the change and I am still getting the same error. I had the same domain working with move base and then created an action interface for my own navigation, whit the same domain just a different launch action.

[gerardcanal]

Hi Karolina, Does the rosservice call /rosplan_knowledge_base/domain/operator_details "name: 'goto_waypoint'" still error? For me it works with the fixed domain.

[karolinajudzentyte]

Hello Gerard, Still getting the same error. Are you using my action interface?

[karolinajudzentyte]

It works. Thank you for your help. It was a spelling error that was changed on the local file but not the robot.

[gerardcanal]

Great! Glad to know it was fixed :-)