Added the simulation for the overarching simulation

[ste93]

Added the simulation with pyrobosim

[BeniaminoSquitieri]

This PR is about all the changes that have been done in order to make all the BT working in simulation folder with pyrobotsim tools.

[charlie1329]

Don't think this causes any issues on my end - for reference the PR for the refine-plan portion of the demo is here: #28

[EnricoGhiorzi]

@ste93 @BeniaminoSquitieri I pushed a small fix to the policies that apparently fixed the base version of the model (main.xml). The three other versions still fail building with AS2FM and I get the following error message:

Value of BT output port output_key must be a Blackboard variable reference.

Any idea what this might be about?

[EnricoGhiorzi]

The last commit fixes the roaml issues I had and makes the models work as intended when verified with Scan.

[EnricoGhiorzi]

Compared to my last test, this branch now breaks verification because of wrong quotes. Model will have lines such as: <assign location="tmp_location" expr="''''"/>.

[EnricoGhiorzi]

Also, trace shows robot drifting to -1000 which I think it was never supposed to do:

Time, Origin, Target, Event,               Values,                                                           parent: string, x: float64, y: float64
1000, world,  NONE,   topic_robot_loc_msg, "[ros_fields__parent: 1, ros_fields__x: -990, ros_fields__y: 0]", 1,              4,          2

[EnricoGhiorzi]

After a little investigation, I think the culprit is in the policy bt_tree.xml:

<NavigateToLocation name="navigate_to_obj" location="'fridge'" />

requesting navigation to 'fridge', while world only handles 'start' and 'table':

        <ros_action_handle_goal name="act_nav" target="handle_nav_request">
            <assign location="goal_id" expr="_action.goal_id" />
            <if cond="_goal.location_id == 'start'">
                <assign location="req_loc" expr="location_start" />
            <elseif cond="_goal.location_id == 'table'" />
                <assign location="req_loc" expr="location_table" />
            <else/>
                <assign location="req_loc.x" expr="-1000" /> <!-- Dummy x value, should never happen! -->
            </if>
            <ros_action_accept_goal name="act_nav" goal_id="goal_id" />
        </ros_action_handle_goal>

I don't know if this has to be fixed changing the BT or including the 'fridge' location in world. Notice that 'fridge' is already included in world_multiple_locations.

[ct2034]

replicated in convince-project/overview#37