World model not loading after system reboot

[AMLeite]

I found a strange behavior and I'm wondering if it is a bug or a system setup issue. Upon installing skiros_simple_uav, as well as all dependencies, I run the launch file and it worked fine. However, after turning off and on my machine, the world model failed to loaded, even though the launch files from skiros and skiros_lib worked fine. Might it be my installation? I'm running Ubunto 16.04 with ros kinetic

[frovida]

I need more info, can you post a screenshot or a printout of the error you are getting?

[AMLeite]

On the world model node it plays and crashes in a loop this:

The terminal first shows this error:

Then goes into an endless loop of this:

Ending it with ctrl+c becomes a matter of very good timing has the world node keeps crashing and restarting because of this.

[frovida]

Seems like you have a corrupted file scene. Try to erase the file skiros_simple_uav/scenes/simple_uav_scene.xml

[AMLeite]

Thank you, that solved the problem. However there is still one question. I'm using skiros for a project, which requires me to add a set of int type variables to each primitive, calculate a new value and then, for each skill, that value will be the highest of the primitives that make it. However I'm failing to understand how to find the primitives that compose each skill. I'm starting to think that I'm seeing this wrong and that each skill is not currently composed of primitives. Is this right or am I missing something?

[RvmiLab]

I can't really understand what you have to do from this little text. Anyhow to answer your question about the primitives: every skill is composed of primitives, but it is not declared explicitly. Since you can use c++ code in the execute function you have complete freedom not to use primitives and implement whatever you want. But If you want to use them, then I copy paste here some code from our real pick pipeline as an example :

int execute()
{
    /** Initialization **/    
    arm_ = getParamHandle()->getParamValue<skiros_wm::Element>("Arm");
    std::vector<skiros_wm::Element> v = getWorldHandle()->getChildElements(arm_.id(), "", concept::Str[concept::Gripper]);
    if(v.size()<=0)
    {
        setProgress(-1, "No gripper found for the arm specified. ");
        return -1;
    }
    gripper_ = v[0];
    this->setProgress("Initializing.");
    v = getWorldHandle()->getChildElements(container_, "", concept::Str[concept::ObservationPose]);
    if(v.size()<=0)
    {
        FINFO("No observation pose for container.");
        return -1;
    }
    else observation_pose_ = v[0];

    skiros::Module arm_motion(this->getModulesHandler(),"arm_motion", this->moduleType());
    skiros::Module registration(this->getModulesHandler(),"registration", this->moduleType());
    skiros::Module gripper(this->getModulesHandler(),"gripper_oc", this->moduleType());

    /** Execute: Close gripper **/
    this->setProgress("Close gripper.");
    gripper.setParam("Gripper", gripper_);
    gripper.setParam("Close", true);
    gripper.setParam("Modality", -1);
    gripper.exe();

    /** MoveArm Over the object **/
    this->setProgress("Move to registration pose.");
    arm_motion.setParam("StartingJointState", "current");
    arm_motion.setParam("DestinationObject", observation_pose_);
    arm_motion.setParam("PlanningSpace", "tool");
    arm_motion.setParam("ApproachDistance", -0.3);
    arm_motion.setParam("ApproachAxis", gripper_.properties("FrontAxis").getValue<std::string>());
    arm_motion.setParam("TrajName", "ObservationTraj");
    arm_motion.exe();
    if(!arm_motion.waitResult())
    {
        setProgress(-1, "Arm motion failed.");
        return -1;
    }
    getWorldHandle()->removeElement(observation_pose_.id());

    // if the object already ahs a registered pose, don't register
    if (!objObject.hasProperty(relation::Str[relation::hasRegisteredShot]))
    {
        /** Execute: Registration **/
        this->setProgress("Registration.");
        registration.setParam("Camera", camera_arm_);
        registration.setParam("Container", container_);
        registration.setParam("PriorInfo", objObject);
        registration.setParam("Register", true);
        registration.exe();
        if(!registration.waitResult())
        {
            setProgress(-3, "Registration failed.");
            revertMovements(arm_motion);
            return -3;
        }
    }
    <...>
   }`

In this example we basically create a connection to 3 primitives, 1 to move the arm, 1 to open the gripper and another one to register the object (meaning aligning a pre-learned model to the point cloud in view, or in other words localizing an object). We close the gripper and move to an observation pose. We assume the observation pose is there, already defined by the Locate skill. Finally we try to register the object model.

After a primitive connection is declared, first thing we set the parameters and then we call the exe function to start it. Exe is not blocking, so if you want to wait for the end you can call "waitResult". Afterward you can also get the result (if any) with "getReturnValue()" . Between the primitives execution you can execute any code you like.

[AMLeite]

I must admit that the non explicit declaration is probably the source of my confusion. Although I believe I'm able to do what I want by simplifying it to only use the new vars I need on the skill itself, I'm still hoping I'll be able to do it at the primitive level (which is the proper way according to the formulation that lead to the need to do this). I'll try to explain better what I wanted to achieve:

The schematic is crude at best, but I think it can properly illustrate the purpose. As can be seem, and as I think skiros itself is structured, there are different skills, each composed by their own set of primitives, which must be executed in proper order (although this is not the best example). This is basically the set up I believe skiros already provides. The difference I'm trying to include is a state that must be calculated for every single primitive and that is determined by faults. In the image, inside the primitive, there are the parameters, execution, other codding they might have. Afterwards there is the addition of six new variables, and two functions to manipulate these variables. State is calculated by a formula that combines the other 5. x to w, however, depend on the scenario simulated by the Fault Simulator I want to connect (I'm assuming the subscription allows for an easy way to produce this). So a generic scenario would produce (x=1, y=5, z=3, t=2, w=6) and the formula would produce state=20 for example. That value would be the state for the primitive Rotate. Now, this process would have to be produced for every single primitive. This is because the skillState would be calculated by assuming the maximum state of all primitives contained in the skill.

I'm not sure if this helped understand what I have to do. As I said, I think I've found a way to do it at the skill level, however I'd like to try to do it at the primitive level since a primitive could be used in more than one skill. So my current question is simply: can this be done with a not explicit declaration?

[frovida]

Yes it can be done. Primitives can call recursively other primitives. What you need to do is to declare a FaultDetection primitive. Then you have two choices: (i) you make FaultDetection a primitive that returns instantly, and you invoke it whenever you want to check the fault inside "accelerate", "rotate", etc.. or (ii) you make FaultDetection an "always active" primitive, meaning that it returns only if a fault is detected, and you invoke it only once in SkillMove, setting a callback when you start it (exe function). If when you finish your "Skill move" the FaultDetection is still running, you can just call the stop function to preempt it. Does this make sense for you?

[AMLeite]

It does, but still I'm having problem locating the primitives. For example the UavDrive skill (defined in the Drive.cpp inside the simple_uav_skills/src folder)

I can't seem to find any primitive being used inside this code. I wanted to add them, but I can't seem to find any kind of file that has the primitives names so I know which names to add in the skiros::Module primitive_name(this->getModulesHandler(),"primitive_name", this->moduleType()); coding. This is from must of my confusion is coming.

[frovida]

You find the primitive names in the plugin.xml files. e.g. in simple_uav_skills the skills are:

<library path="libsimple_uav_skills">
    <class  name="drive_uav" type="skiros_skill::DriveUav" base_class_type="skiros_skill::SkillBase">
      <description>
      Fake driving skill
      </description>
    </class>
    <class name="pick_uav" type="skiros_skill::PickUav" base_class_type="skiros_skill::SkillBase">
      <description>
      Fake picking skill
      </description>
    </class>
    <class name="place_uav" type="skiros_skill::PlaceUav" base_class_type="skiros_skill::SkillBase">
      <description>
      Fake placing skill
      </description>
    </class>
    <class name="fly_altitude" type="skiros_skill::FlyAltitude" base_class_type="skiros_skill::SkillBase">
      <description>
      Fake flying skill
      </description>
    </class>
    <class name="land" type="skiros_skill::Land" base_class_type="skiros_skill::SkillBase">
      <description>
      Fake flying skill
      </description>
    </class>
</library>```