Unexpected error with setForwardedProperties

[bostoncleek]

I modified the pick/place demo to include the SimpleGrasp stage using this demo as a guide. The reason is because SimpleGrasp is the only stage that uses the setForwardedProperties() functionality which I am trying to use.

The modification made to the demo is bellow.

Stage* attach_object_stage = nullptr;  // Forward attach_object_stage to place pose generator
{
  auto grasp = std::make_unique<SerialContainer>("pick object");
  t.properties().exposeTo(grasp->properties(), { "eef", "hand", "group", "ik_frame" });
  grasp->properties().configureInitFrom(Stage::PARENT, { "eef", "hand", "group", "ik_frame" });

  {
    // Sample grasp pose
    auto stage = std::make_unique<stages::GenerateGraspPose>("generate grasp pose");
    stage->properties().configureInitFrom(Stage::PARENT);
    stage->properties().set("marker_ns", "grasp_pose");
    stage->setPreGraspPose(hand_open_pose_);
    stage->setObject(object);
    stage->setAngleDelta(M_PI / 12);
    stage->setMonitoredStage(current_state_ptr);  // Hook into current state

    // Compute IK
    auto wrapper = std::make_unique<stages::SimpleGrasp>(std::move(stage));
    wrapper->setMaxIKSolutions(8);
    wrapper->setIKFrame(grasp_frame_transform_, hand_frame_);

    // Pick Container (approach/lift)
    auto pick = std::make_unique<stages::Pick>(std::move(wrapper));
    pick->setProperty("eef", eef_name_);
    pick->setProperty("object", std::string("object"));
    pick->setProperty("eef_frame", hand_frame_);

    geometry_msgs::TwistStamped approach;
    approach.header.frame_id = hand_frame_;
    approach.twist.linear.z = 1.0;
    pick->setApproachMotion(approach, approach_object_min_dist_, approach_object_max_dist_);

    geometry_msgs::TwistStamped lift;
    lift.header.frame_id = world_frame_;
    lift.twist.linear.z = 1.0;
    pick->setLiftMotion(lift, lift_object_min_dist_, lift_object_max_dist_);

    grasp->insert(std::move(pick));

    t.add(std::move(grasp));
  }
}

The error I get is:

terminate called after throwing an instance of 'std::runtime_error'
  what():  Property 'grasp': trying to set undeclared property 'grasp' with NULL value
in stage 'compute ik': undeclared
in stage 'grasp': undeclared
in stage 'pick': undeclared
in stage 'pick object': undeclared
in stage 'pick_task': undeclared

I would appreciate any help in trying to get the SimpleGrasp stage to work or using setForwardedProperties().

[v4hn]

Sorry, I only found this issue again just now. The short answer is that you are currently rather on your own if you rely on forwarded properties in your pipeline. In theory this should work, but @rhaschke never finished a good example for this and it is rather likely you have to modify some code to get it to work for anything really.

It's one of many open construction sites in the framework.

[v4hn]

Turns out I actually have to address this problem for one of my own Tasks and we will probably have to modify the Stage interface to address this.

For a temporary solution, you can explicitly declare the forwarded property in each stage and configure it to initialize from INTERFACE:

s->properties().declare<GraspType>("grasp");
s->properties().configureInitFrom(Stage::INTERFACE, {"grasp"});
s->setForwardedProperties({"grasp"});

[rhaschke]

I think you are just missing the definition of the grasp pose in your GenerateGraspPose stage. Comparing with the Baxter example, you are missing this line: stage->setGraspPose("closed");

This grasp pose is exposed to the solution's interface state (and then to subsequent stages) here: https://github.com/ros-planning/moveit_task_constructor/blob/62813f33f79fc4a91647b439a8e894e95e7c02ba/core/src/stages/generate_grasp_pose.cpp#L140

but it's not defined in your GenerateGraspPose stage.