Exercise 2¶
In this exercise you will use our MDP/LTL planning framework to get the robot to perform 3D mapping sweeps of a simple. This example connects many of the different elements of our robot system, and may seem complex at first. If something is unclear, or you want more information, please just ask.
Background¶
You must first run some basic elements from the STRANDS system. You
should ideally run each of these in a separate terminal where you have
sourced both the ROS and your local workspace setup.bash
files, as
described in tutorial_prep.md.
MongoDB Store¶
(If you still have the database running from Exercise 1 you can skip this step)
First, check the db
directory exists (which you should’ve created
following tutorial_prep.md). The following
should not list any files or report an error:
ls `rospack find planning_tutorial`/db
If that is ok, then launch mongodb_store using that directory as its data path:
roslaunch mongodb_store mongodb_store.launch db_path:=`rospack find planning_tutorial`/db
MORSE Simulation¶
In another terminal, launch our object simulation taken from the ALOOF Project.
roslaunch strands_morse aloof_morse.launch
If you press the ‘h’ key in MORSE you can see a list of available keyboard commands.
MDP Planning¶
Next launch the MDP-based task executive system in (yet another!) new terminal:
roslaunch mdp_plan_exec mdp_plan_exec_extended.launch
3D Mapping Nodes¶
Our 3D mapping framework makes use of an approach called meta-rooms which builds 3D maps at waypoints in the environment. Before you run this for the first time you need to create somewhere for the system to store maps. Do this with the following command:
mkdir ~/.semanticMap
If this was successful, you can launch the meta-room nodes with the following command:
roslaunch planning_tutorial meta_rooms.launch
Exercise 2a¶
In Exercise 1 you exploited the fact that the execution framework automatically creates an MDP for navigation across the topological map. In this exercise we will extend this MDP with additional actions which connect ROS actionlib servers to actions in the MDP.
In this part we will walk through an example where the outcome of the invocation of an action server is tied to a non-deterministic outcome of an action in an MDP. After showing you how to do this, the next step will be for you to edit the file to change how the action is encoded in the MDP.
All of the code for this part of the exercise is in
planning_tutorial/script/sweep_at_waypoints.py
so only the important
fragments will be included here.
The task we will undertake is to trigger the /do_sweep
action server
with the argument medium
(signifying the number of views to take in
the sweep) at a set of waypoints in the topological map. The key idea is
that we associate the action server with an MDP action which we add to
the navigation MDP. To make sure we only complete the action once, we
connect the successful completion of this action to a state variable in
our MDP specification, which can be used in pre- and post-conditions for
our action.
We start by creating a name for our action. As we care about the success of an action at a waypoint, we need a different action for each waypoint. This is captured in the naming of the action, which should be different for each waypoint, i.e.
action_name = 'do_sweep_at_' + waypoint
Next we create a the MDP state variable for tracking the success of the
action. This state variable will form part of our goal statement, e.g.
if we have the state variable executed_do_sweep_at_WayPoint1
the
goal to eventually make this true would be
(F executed_do_sweep_at_WayPoint1=1)
.
state_var_name = 'executed_' + action_name
# Create the state var object, initialise to 0 (false)
state_var = MdpStateVar(name = state_var_name, init_val = 0, min_range = 0, max_range = 1)
Following this we need to encode the possible outcomes of the action and
the way they can change the state in the MDP. Although there is no
restriction on the number of outcomes an action can have (over one), we
will use two: succeeded or not succeeded. The /do_sweep
action
reports its outcomes when it completes, so we will use this return
signal to tell the MDP which outcome occurred (allowing it to update its
internal state correctly). Each outcome has a probability of occurring
(which is used when creating a policy to solve the MDP), and a
probability distribution over how long it might take to reach the
outcome. In this example we make up these values, but in our full system
we can learn them from experience at execution time.
successful_outcome = MdpActionOutcome(
# the probability of this outcome occurring
probability = 0.99,
# the effects of this action on the MDP state, in this case setting the state variable to 1
post_conds = [StringIntPair(string_data = state_var_name, int_data = 1)],
# how long it will take to reach this outcome, and with what probability
duration_probs = [1.0],
durations = [240.0],
# And the action server outcome which will tell us if this outcome occurred. In this case if the action server returns with SUCCEEDED
status = [GoalStatus(status = GoalStatus.SUCCEEDED)])
Below is a similar encoding for the unsuccessful outcome, i.e. the cases where the action server reports that it has aborted or was preempted.
unsuccessful_outcome = MdpActionOutcome(probability = (1 - successful_outcome.probability),
post_conds = [StringIntPair(string_data = state_var_name, int_data = 0)],
duration_probs = [1.0],
durations = [120.0],
status = [GoalStatus(status = GoalStatus.ABORTED), GoalStatus(status = GoalStatus.PREEMPTED)])
The final step is to link the MDP action to the actionlib server. This
is done in the code below. The action_server
argument is used to
configure the action server client, and the add_string_argument
call
is used to embed the parameters for the call into the action itself (see
strands_executive_msgs.mdp_action_utils
for more options).
# Finally we tie all this together to an actionlib server
action = MdpAction(name=action_name,
# This is the actual action server topic to be used
action_server='/do_sweep',
# The action will only be attemped if the preconditions are satisfied. In this case we can't have succeeded in the action before
pre_conds=[StringIntPair(string_data=state_var_name, int_data=0)],
# These are the possible outcomes we defined above
outcomes=[successful_outcome, unsuccessful_outcome],
# And this is where we can execute the action.
waypoints = [waypoint])
add_string_argument(action, 'medium')
For the purposes of the tutorial you should understand the overall approach we are taking for this, and be able to map this understanding into the code. To see it working you can run:
rosrun planning_tutorial sweep_at_waypoints.py
Feel free to edit the code as you wish and play around with the file.
You could add more waypoints to the list, or add additional LTL
navigation goals (as in Exercise 1) to the string
mdp_spec.ltl_task
to change the robot’s behaviour (e.g. get it to do
a sweep without every visiting a given waypoint).
Exercise 2b¶
To successfully complete the following you need to be competent with both Python and ROS. If you’re not comfortable with these, please pair up with someone who is.
To test your understanding, create a copy of
planning_tutorial sweep_at_waypoints.py
and re-write it such that
instead of completing a sweep at every waypoint, the problem is to
simply complete a single sweep at any point at the map. However you
should be able to specify that the chance of the action succeeding is
different at different waypoints (with you inventing the numbers), and
this should result in the robot choosing the waypoint with the highest
chance of success for the sweep.
Exercise 2c¶
What happens when you combine your answer to 2b with the ability to change the probabilities of success on the edges of the topological map? Talk to one of the lecturers for help with getting this running if you want to try it.
Original page: https://github.com/strands-project/planning_tutorial/blob/indigo-devel/doc/exercise_2.md