Moveit Task constructor properties propagation

edit

ROS distro : kinetic

Moveit task constructor branch : master

Moveit branch : master

Hi,

I don't manage to really understand how the properties propagate through the different stages. In a simple example, I want to define some task properties (like in the demo) : group, eef, object, etc. Then I create a simple pick :

pickTask.setProperty("group",planGroup);
pickTask.setProperty("eef","right_gripper");
pickTask.setProperty("pregrasp","right_open");
pickTask.setProperty("grasp","right_close");
eef = "right_gripper";
ikFrame = "r_gripper_tool_frame";


//Start state
Stage* current_state = nullptr;
auto initial = std::make_unique<stages::CurrentState>("current state");
current_state = initial.get();
pickTask.add(std::move(initial));

{
    // connect to pick
    stages::Connect::GroupPlannerVector planners = {{eef, pipeline}, {planGroup, pipeline}};
    auto connect = std::make_unique<stages::Connect>("connect", planners);
    connect->properties().configureInitFrom(Stage::PARENT);
    pickTask.add(std::move(connect));
}

{
    // grasp generator
    auto grasp_generator = std::make_unique<stages::GenerateGraspPose>("generate grasp pose");
    grasp_generator->setAngleDelta(M_PI/2);
    //grasp_generator->setPreGraspPose("right_open");
    //grasp_generator->setGraspPose("right_close");
    //grasp_generator->setProperty("object", object);
    grasp_generator->setMonitoredStage(current_state);

    auto grasp = std::make_unique<stages::SimpleGrasp>(std::move(grasp_generator));
    Eigen::Isometry3d tr = Eigen::Isometry3d::Identity();
    //tr.translation() = Eigen::Vector3d(0.0,0.0,0.0);
    grasp->setIKFrame(tr, ikFrame);
    grasp->setMaxIKSolutions(10);

    // pick container, using the generated grasp generator
    auto pick = std::make_unique<stages::Pick>(std::move(grasp),"pick");
    //pick->setProperty("eef", "right_gripper");
    //pick->setProperty("group","right_arm");
    //pick->setProperty("object", object);
    geometry_msgs::TwistStamped approach;
    approach.header.frame_id = ikFrame;
    approach.twist.linear.x = 1.0;
    pick->setApproachMotion(approach, 0.01, 0.10);

    geometry_msgs::TwistStamped lift;
    lift.header.frame_id = "base_footprint";
    lift.twist.linear.x =  0.0;
    lift.twist.linear.y =  0.0;
    lift.twist.linear.z =  1.0;
    pick->setLiftMotion(lift, 0.05, 0.10);
    current_state = pick.get();
    pickTask.add(std::move(pick));

What would be the best way to use the task properties in the different stages without redefinition (with multiple setProperty for each stages) ? I saw in the demo that the exposeTo function is used to take properties defined in the task to the propertymap of the stage. I tried with exposeTo but the properties doesn't seems to propagate well in the SimpleGrasp's computeIk substage. Let me precise what I am talking about. If I modify the grasp_generator stage by adding :

pickTask.properties().exposeTo(grasp_generator->properties(), { "group", "eef","pregrasp","grasp","object" });
grasp_generator->setForwardedProperties({ "group", "eef", "pregrasp", "grasp", "object" });

I get an error :

generate grasp pose: Property 'pregrasp': undefined

in stage 'compute ik': undeclared

in stage 'grasp': undeclared

in stage 'pick': undeclared

in stage 'myTask': defined

How do I propagate the property in nested stages (Pick is taking simple_grasp and simple_grasp is taking grasp generator) ?

Thank you for the help,

regards,