rosin-project / metacontrol_sim Goto Github PK

Following specification in meta-control/mc_mros_reasoner#11

• Implement MvpRconfiguration.action
• Implement node Rosgraph_manipulator
  • Implement action server
  • Implement launch configuration
  • retrieve move_base goal when it is reconfigured see #10

This is what we discussed today.
Currently the reconfiguration uses a single launch file and a launch argument to select amongst predefined param configs for move base.
See:

What we need is to launch the launch files autogenerated from the .rossystem files of each configuration.

These files have the name and they are in a package both with the name of the configuration, which can be retrieved from the goal field desired_configuration_name
see: metacontrol_msgs/action/MvpReconfiguration.action

These lines also need to be changed:

How to came up with a normalized value for the performance observer.

Publish

P = Closest distance to an object in the direction of motion
Pr = Closest distance to an object in the direction of motion - inflation_radius

Information should come from the laser scanner

With commit ed75d10 the following error occurs:

RLException: error loading <rosparam> tag: 
	file does not exist [/home/gus/metacontrol_ws/src/metacontrol_sim/yaml/rosgraph_manipulator_config.yaml]
XML is <rosparam ns="rosgraph_manipulator" command="load" file="$(find metacontrol_sim)/yaml/rosgraph_manipulator_config.yaml"/>
The traceback for the exception was written to the log file

This is due metacontrol_sim/yaml/rosgraph_manipulator_config.yaml being removed.

I think the solution is to replace in rosgraph_nodes.launch.xml the line:

<rosparam ns="rosgraph_manipulator" command="load" file="$(find metacontrol_sim)/yaml/rosgraph_manipulator_config.yaml"/>

With:

<rosparam ns="rosgraph_manipulator" command="load" file="$(find mc_rosgraph_manipulator)/config/rosgraph_manipulator_config.yaml">

If this is right I can open a PR

I tested the simulation and it works, great job @marioney 🎉🎉
The only problem I have is that the real time rate of gazebo is super slow (obviously the problem is my laptop, I usually use my workstation but these days I don't have access to it). Is it ok for you if I create a simplified version of the simulation? Probably just remove the controllers of the yumi will be ok.

Short test protocol:

Installation: https://github.com/rosin-project/metacontrol_sim/blob/1adaafb28d5a92acca70e175955c35e1204418f3/README.md#setup-using-wstool

Command: roslaunch metacontrol_sim ridgeback_yumi_world.launch gui:=true

Test:
Terminal A:

rostopic pub /ridgeback_velocity_controller/cmd_vel geometry_msgs/Twist "linear:
  x: 0.0
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 1.0" -r 10

Terminal B:
rostopic echo /ridgeback_velocity_controller/odom

The robot didn't move on gazebo but probably it is related to my computer...

Just 2 workstations (goals) - try with the factory map (desired), but if problematic use the map we have.

Should follow the definition in rosin-project/metacontrol_nav#4

Needs:

• Data logging tool #25
• Create factory environment
• Define Goal positions - a set of 3 values
• Define Initial positions - a set of 3 values
• Create map - only one is going to be used
• Define launch configuration: - a set of 3 configurations (Fast / Standard / Safe)
• Create control script for the experiments: #29

This happens in 35cd0dc when using the updated launchfile:

$ roslaunch metacontrol_sim metacontrol_sim_reasoner.launch
[...]
[INFO] [1600862917.455973, 7.662000]: QA value received!        TYPE: safety    VALUE: 0.490094699729

FGs:
fg_f3_v3_r1     objective:  kb.o_navigateA      status:  None   FD:  kb.f3_v3_r1        QAvalues:  [('safety', 0.490094699729), ('energy', 0.415905276934)]

OBJECTIVE       |  STATUS       |  NFRs
[WARN] [1600862917.457630, 7.663000]: QA value received for             TYPE: safety    VALUE: 0.702318014465
o_navigateA     |   INTERNAL_ERROR      |   [('energy', 0.5), ('safety', 0.8)]
[WARN] [1600862917.458305, 7.663000]: Objectives in status ERROR: ['o_navigateA']
[INFO] [1600862917.460224, 7.663000]: QA value received!        TYPE: safety    VALUE: 0.702318014465
[WARN] [1600862917.461828, 7.663000]: QA value received for             TYPE: safety    VALUE: 0.79356236787
[INFO] [1600862917.466717, 7.664000]:   >> Finished MAPE-K ** ANALYSIS **
[INFO] [1600862917.468758, 7.666000]:   >> Started MAPE-K ** PLAN adaptation **
[INFO] [1600862917.470665, 7.666000]: => Reasoner searches FD for objective: o_navigateA
[INFO] [1600862917.471677, 7.666000]: QA value received!        TYPE: safety    VALUE: 0.79356236787
== FunctionDesigns available for obj: %s ['f1_v1_r1', 'f1_v1_r2', 'f1_v1_r3', 'f1_v2_r1', 'f1_v2_r2', 'f1_v2_r3', 'f1_v3_r1', 'f1_v3_r2', 'f1_v3_r3', 'f2_v1_r1', 'f2_v1_r2', 'f2_v1_r3', 'f2_v2_r1', 'f2_v2_r2', 'f2_v2_r3', 'f2_v3_r1', 'f2_v3_r2', 'f2_v3_r3', 'f3_v1_r1', 'f3_v1_r2', 'f3_v1_r3', 'f3_v2_r1', 'f3_v2_r2', 'f3_v2_r3', 'f3_v3_r1', 'f3_v3_r2', 'f3_v3_r3']
Objective NFR ENERGY: %s [kb.nfr_energy, kb.nfr_safety]
== Checking FDs for Objective with NFRs type: %s and value %s  energy 0.5
No FDs meetf NFRs
*** OPERATOR NEEDED, NO SOLUTION FOUND ***
[ERROR] [1600862917.475400, 7.668000]: No FD found to solve Objective o_navigateA
[...]

Maybe it is due to the particular scenario used in this launchfile, can you please check @marioney

Problem running

roslaunch metacontrol_sim ridgeback_yumi_world.launch

Looks like the correct directory is:
/home/parallels/rosin_paper_ws/src/metacontrol_sim/launch/includes/description_ridgeback_yumi.launch.xml

Move rosgraph_manipulator to a separate repo, because it is a metacontrol asset that should be "experiment/system" agnostic, but platform specific for ROS1

(issue created from #31 (comment))
To test the metacontroller I need to be able to launch the simulation:

• for only 1 goal
• selecting start and goal poses
• Perturbation: I need to add a perturbation, choosing which one. Whatever is easier:
  • Add unknown obstacles
  • Increase energy consumption

Because there are two sections: "Setup using wstool" and "Manually setup", I though I could simply follow one of them, so I did with wstool.
But you actually need to do BOTH.

I propose to have a single "Installation and setup section", and introduce both sets of instructions with a sentence in normal text, not as a header.

These are the topics to which the observer needs to subsbscirbe:

topics = [("/odom", Odometry), ("/imu/data", Imu), ("/d_obstacle", Float32)]
The code to the observer is here

SHould we specify a minimal test for our MVP? We could follow the example of CMU
and then extend it for all the experiments in the paper.
what do you think @darkobozhinoski ?

define a launchfile for the world in the MVP scenario

Do we need a header timestamp in all our messages for data logging?

• List all publishers we have created that we need to log (from #25 (comment))
• Add header in .msg if needed
• Make sure messages published have a correct timestamp initialized

quick solution for MVP to the more general issue meta-control/mc_mros_reasoner#12

For this we will use navigation goal poses from #11

As the items from #26 are completed, I can now run the initial experiments (27 Runs: 3 initial points, 3 goals and 3 navigation configurations)

@darkobozhinoski @chcorbato The perturbations described in rosin-project/metacontrol_nav#4 are not yet implemented, do we need them for the initial set of experiments?

@darkobozhinoski is the safety observer working? or should we just use the qa_energy?

In order to manage component failure, and reconfiguration, a Kinect model needs to be added to the ridgeback.

@chcorbato should we have a different field of view / range or use the same as the laser?

Since we're using fake_localization it only needs to be configured for navigation

When launching the simulation after following all the instructions and fixing the error with the xacro file (see pull request, branch carlos_test), I get the following error,

[ INFO] [1586868553.118651086]: gazebo_ros_control plugin is waiting for model URDF in parameter [robot_description] on the ROS param server.
[ERROR] [1586868553.343810232]: No p gain specified for pid.  Namespace: /gazebo_ros_control/pid_gains/yumi_joint_1_r
[...]

The model of safety is taken from this reference.

Safety observer is defined here.

Run-time safety is calculated as follows:
S= {
1, if P > d_break;
P_r/d_break, otherwise; …. (Eq1)
}
where:

• P - a distance from a current location of the robot to the closest obstacle in the robot direction.
• P_r = P - inflation_radius
• d_break is the breaking distance of the robot before hitting an obstacle;

Furthermore:

• P is computed from the laser scans;
• d_break = v^2*r /(2 × a_robot) … (Eq2) where:
  * v - the current speed of the robot;
  * a_robot - the acceleration of the robot;
  * r - Radius of the robot ;

This happens very frequently, especially with the EnergyObserver.

CONTEXT

 ./run_single_sim.sh -i 2 -g 2 -n standard -r true -p 1.3

OUTPUT

[INFO] [1590215602.961954, 0.000000]: Controller frequency: 1.0
energy
EnergyQualityObserver
[INFO] [1590215603.004127, 0.000000]: Waiting for service...
[INFO] [1590215603.011966, 0.000000]: Waiting for service...
Could not start EnergyQualityObserver
The load observer service call is completed!
safety
SafetyQualityObserver
[INFO] [1590215603.149441, 0.000000]: Waiting for service...
Could not start SafetyQualityObserver
The load observer service call is completed!

When running ./run_single_sim.sh -p 1 -g 2 -n "f1_v1_r1" -o 2 -r "true" -p 1.2 in the experiments repo, the following Error appears. This do not affect the normal execution of the mission.

[ERROR] [1596639458.195704258, 3.140000000]: GetModelProperties: model [unknown_obstacle1] does not exist
[ERROR] [1596639458.380327582, 3.270000000]: GetModelProperties: model [unknown_obstacle2] does not exist

We need an application logic to manage the goals, but maybe for the MVP we can simply send the goal in the launchfile, what do you think @marioney ?

This happens in 35cd0dc when using the updated launchfile:

$ roslaunch metacontrol_sim metacontrol_sim_reasoner.launch
[...]
FGs:
fg_f3_v3_r1     objective:  kb.o_navigateA      status:  None   FD:  kb.f3_v3_r1        QAvalues:  []
[...]

When echoing /diagnostics:

$ rostopic echo /diagnostics 
WARNING: no messages received and simulated time is active.
Is /clock being published?

It seems to be the same issue than in meta-control/mc_mros_reasoner#89

Currently, there are some ad-hoc code to provide each of the nodes in the metacontrol MAPE-K loop the information about configuration. We need to have a general solution to sync this information, which includes:

• existing configurations (aka FDs in tomasys, aka rossystem files in ROS model), that is the KB
• the current objective and associated NFR (obj+NFRs), and
• the current configuration deployed - see meta-control/mc_mros_reasoner#28

Let's analyze this per metacontrol node in the MAPE-K loop (checkboxes indicate the design stated next to it has been implemented):

Monitor: rosgraph_monitor

ROSGraph monitor

It monitors that the configuration of nodes/components is the desired_configuration

• use a component model: currently it uses the initial rossystem model at deployment and the current rossystem generated by ros_graph_parser
• TODO (not for paper): update dynamically at runtime the desired_rossystem with the new configuration deployed by metacontrol

Objectives observers

They detect the status of the running objectives (are they in error because the task management reports so?)

• TODO: required for many scenarios (move_base cannot plan route in navigation, tag not detected in Cheops)
• We could monitor action goal feedback, if we associate objective <-> ROS action goal
• This could be "merged" with QA Observers

QA Observers:

• TODO (not for paper): They need to know for which objective they are reporting QA values, either by knowing the objective or by knowing the configuration that is achieving those values.
  Currently: only one objective and 1 configuration assumed, mros1_reasoner does the association QAvalue-Objective
• Deployment time: they are automatically generated using RosModel tooling (see @ipa-nhg please add your pull request on autom creating observer models) and a template.
  • TODO (not for paper): currently for MVP the application launchfile including the observers is a manual one by @marioney rosgraph_nodes.launch.xml in

    metacontrol_sim/launch/MVP_metacontrol_world.launch

    Line 61 in 1a8c386

    <include file="$(find metacontrol_sim)/launch/includes/rosgraph_nodes.launch.xml" />

  • TODO (not for paper): generalize/automate the addition of the QA observation model to the observer (ilnk to Nadia's issue on this)
• Dynamically at runtime:
  • for a given obj+NFRs they do not need to change.
  • TODO (not for paper): for dynamic objestives+NFRs they need to adapt, e.g. metacontrol could deploy dynamically new

NOTE: observers need to be activated (see code below). This needs to be "automated". mros1_reasoner could do that, contributing to address the deployment and the runtime TODOs

IMPLEMENTATION NODE for Monitoring:
Currently the monitoring interface M-A is via ROS diagnostics messages
This is causing some problems and does not provide good semantics

• TODO (not paper, but IMP): implement a specific metacontrol monitoring interface in a new ROS message type

Analyse, Plan and KB - mros1_reasoner

• Deployment time:
  • load the application ontology file
  • initialize obj+NFRs and current configuration (see meta-control/mc_mros_reasoner#28)
  • currently: initialized in reasoning code: https://github.com/rosin-project/mros1_reasoner/blob/5263ad6da4a7507754caf6dbf0068e774c223b65/mros1_reasoner/scripts/init_models.py#L46
• TODO: separate it from the reasoning code, i.e. separate application-specific. Options:
• Use params see meta-control/mc_mros_reasoner#28
• init them from RosModel in the KB
  • model them in the RosModel: see @ipa-nhg system.rossystem params
  • load them in the KB

Execute - rosgraph_manipulator.py

• It requires the Knowledge of configurations to execute the adaptations:
  • For the moment Metacontrol implementation considers configurations/FDs as atomic adaptation elements, so it only needs to know how to start and kill configurations.
    • Currently kill&launch model with launchfiles
      • launchfiles used are autom generated from models
      • rosgraph_manipulator.py uses a YAML file with the configurations name
        • TODO: autom sync configurations with KB, so as not to have that info twice. Options:
          • automatically generate the YAML file
          • use the KB (ontology). Options
            • move Execute to mros1_reasoner to use the KB
            • rosgraph_manipulator.py also accesses the ontology.
              • using Owlready2 requires to move this to metacontrol ws with python3. We could simply load the same owl file. Problem: can be out of sync if we use dyn info at runtime
              • using distributed KB in ROS, e.g. ARMOR
    • TODO: consider more complex reconfigurations models than kill&launch: some nodes need to be put in a safe state before killing, some states could need to be saved, etc

I have tested d92e96e and mros1_reasoner gets stuch waiting for the result of the reconfiguration action.
I guess something goes wrong with rosgraph_manipulator
I have tested it launching MVP_ commenting the following line:

Observer for Objective error: needed for scenarios 3 in yellow table

• Should check if there's an error when the route is not available.
  - Path length 0
• Check move_base/result topic
• Should check if the robot is blocked (not moving after X seconds)

Objective should publish: "true" if there's an error
"false" if the system is running OK.

Also create another another observer on the rosgraph_observer to send it to the reasoner

https://docs.google.com/document/d/1BaOxo7_WQ2fUUUWIyw5pE_K_q6sErQJXge5VeFjyhgQ/edit#bookmark=id.4rg7xmp249au

Create service client to call the load observer service and activate the QA observer and add it to the MVP Launcher

Fake one but based on Harsh observer
Requirement is to output the proper diagnostic message

Create script to launch the experiments as defined by #26 and the logging tool of #25

Add Publisher for current power load as Float32, to the battery load controller

In this package in the MVP branch, the dependency ridgeback_gazebo is missing in package.xml.

This results in a working simulation, except that the robot cannot move

This is needed for #26