├── .gitignore ├── CMakeLists.txt ├── LICENSE.md ├── README.md ├── config └── config.rviz ├── launch ├── action_example.launch ├── follow_pose_example.launch ├── main.launch ├── planning_example.launch ├── simple_params_example.launch └── turtlesim_example.launch ├── owl ├── planning_scene.turtle └── turtlesim_robot_description.owl ├── package.xml ├── setup.py └── src └── skiros2_examples ├── __init__.py ├── action_example ├── __init__.py ├── skill_client.py └── test_action_server ├── follow_pose_example ├── __init__.py ├── follow_pose_primitives.py └── follow_pose_skills.py ├── planning_example ├── __init__.py └── planning_skills.py ├── simple_params_example ├── __init__.py ├── simple_params_primitives.py └── simple_params_skills.py └── turtlesim_example ├── __init__.py ├── turtlesim_primitives.py └── turtlesim_skills.py /.gitignore: -------------------------------------------------------------------------------- 1 | *.user 2 | *.pyc 3 | *.creator 4 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(skiros2_examples) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | rospy 6 | skiros2_msgs 7 | ) 8 | 9 | catkin_python_setup() 10 | 11 | catkin_package( 12 | CATKIN_DEPENDS rospy skiros2_msgs 13 | ) 14 | 15 | ########### 16 | ## Build ## 17 | ########### 18 | 19 | include_directories( 20 | ${catkin_INCLUDE_DIRS} 21 | ) 22 | 23 | catkin_install_python(PROGRAMS 24 | src/skiros2_examples/action_example/test_action_server 25 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 26 | ) 27 | -------------------------------------------------------------------------------- /LICENSE.md: -------------------------------------------------------------------------------- 1 | SkiROS2 examples - Collection of examples for skiros2 2 | 3 | Software License Agreement (L-GPL License) 4 | 5 | Copyright (C) 2018 RiACT ApS 6 | 7 | Authors: Francesco Rovida, Bjarne Grossmann 8 | 9 | This program is free software: you can redistribute it and/or modify 10 | it under the terms of the GNU Lesser General Public License as published by 11 | the Free Software Foundation, either version 3 of the License, or 12 | (at your option) any later version. 13 | 14 | This program is distributed in the hope that it will be useful, 15 | but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | GNU General Public License below for more details. 18 | 19 | ----------------------------------------------------------------------- 20 | 21 | GNU LESSER GENERAL PUBLIC LICENSE 22 | Version 3, 29 June 2007 23 | 24 | Copyright (C) 2007 Free Software Foundation, Inc. 25 | Everyone is permitted to copy and distribute verbatim copies 26 | of this license document, but changing it is not allowed. 27 | 28 | 29 | This version of the GNU Lesser General Public License incorporates 30 | the terms and conditions of version 3 of the GNU General Public 31 | License, supplemented by the additional permissions listed below. 32 | 33 | 0. Additional Definitions. 34 | 35 | As used herein, "this License" refers to version 3 of the GNU Lesser 36 | General Public License, and the "GNU GPL" refers to version 3 of the GNU 37 | General Public License. 38 | 39 | "The Library" refers to a covered work governed by this License, 40 | other than an Application or a Combined Work as defined below. 41 | 42 | An "Application" is any work that makes use of an interface provided 43 | by the Library, but which is not otherwise based on the Library. 44 | Defining a subclass of a class defined by the Library is deemed a mode 45 | of using an interface provided by the Library. 46 | 47 | A "Combined Work" is a work produced by combining or linking an 48 | Application with the Library. The particular version of the Library 49 | with which the Combined Work was made is also called the "Linked 50 | Version". 51 | 52 | The "Minimal Corresponding Source" for a Combined Work means the 53 | Corresponding Source for the Combined Work, excluding any source code 54 | for portions of the Combined Work that, considered in isolation, are 55 | based on the Application, and not on the Linked Version. 56 | 57 | The "Corresponding Application Code" for a Combined Work means the 58 | object code and/or source code for the Application, including any data 59 | and utility programs needed for reproducing the Combined Work from the 60 | Application, but excluding the System Libraries of the Combined Work. 61 | 62 | 1. Exception to Section 3 of the GNU GPL. 63 | 64 | You may convey a covered work under sections 3 and 4 of this License 65 | without being bound by section 3 of the GNU GPL. 66 | 67 | 2. Conveying Modified Versions. 68 | 69 | If you modify a copy of the Library, and, in your modifications, a 70 | facility refers to a function or data to be supplied by an Application 71 | that uses the facility (other than as an argument passed when the 72 | facility is invoked), then you may convey a copy of the modified 73 | version: 74 | 75 | a) under this License, provided that you make a good faith effort to 76 | ensure that, in the event an Application does not supply the 77 | function or data, the facility still operates, and performs 78 | whatever part of its purpose remains meaningful, or 79 | 80 | b) under the GNU GPL, with none of the additional permissions of 81 | this License applicable to that copy. 82 | 83 | 3. Object Code Incorporating Material from Library Header Files. 84 | 85 | The object code form of an Application may incorporate material from 86 | a header file that is part of the Library. You may convey such object 87 | code under terms of your choice, provided that, if the incorporated 88 | material is not limited to numerical parameters, data structure 89 | layouts and accessors, or small macros, inline functions and templates 90 | (ten or fewer lines in length), you do both of the following: 91 | 92 | a) Give prominent notice with each copy of the object code that the 93 | Library is used in it and that the Library and its use are 94 | covered by this License. 95 | 96 | b) Accompany the object code with a copy of the GNU GPL and this license 97 | document. 98 | 99 | 4. Combined Works. 100 | 101 | You may convey a Combined Work under terms of your choice that, 102 | taken together, effectively do not restrict modification of the 103 | portions of the Library contained in the Combined Work and reverse 104 | engineering for debugging such modifications, if you also do each of 105 | the following: 106 | 107 | a) Give prominent notice with each copy of the Combined Work that 108 | the Library is used in it and that the Library and its use are 109 | covered by this License. 110 | 111 | b) Accompany the Combined Work with a copy of the GNU GPL and this license 112 | document. 113 | 114 | c) For a Combined Work that displays copyright notices during 115 | execution, include the copyright notice for the Library among 116 | these notices, as well as a reference directing the user to the 117 | copies of the GNU GPL and this license document. 118 | 119 | d) Do one of the following: 120 | 121 | 0) Convey the Minimal Corresponding Source under the terms of this 122 | License, and the Corresponding Application Code in a form 123 | suitable for, and under terms that permit, the user to 124 | recombine or relink the Application with a modified version of 125 | the Linked Version to produce a modified Combined Work, in the 126 | manner specified by section 6 of the GNU GPL for conveying 127 | Corresponding Source. 128 | 129 | 1) Use a suitable shared library mechanism for linking with the 130 | Library. A suitable mechanism is one that (a) uses at run time 131 | a copy of the Library already present on the user's computer 132 | system, and (b) will operate properly with a modified version 133 | of the Library that is interface-compatible with the Linked 134 | Version. 135 | 136 | e) Provide Installation Information, but only if you would otherwise 137 | be required to provide such information under section 6 of the 138 | GNU GPL, and only to the extent that such information is 139 | necessary to install and execute a modified version of the 140 | Combined Work produced by recombining or relinking the 141 | Application with a modified version of the Linked Version. (If 142 | you use option 4d0, the Installation Information must accompany 143 | the Minimal Corresponding Source and Corresponding Application 144 | Code. If you use option 4d1, you must provide the Installation 145 | Information in the manner specified by section 6 of the GNU GPL 146 | for conveying Corresponding Source.) 147 | 148 | 5. Combined Libraries. 149 | 150 | You may place library facilities that are a work based on the 151 | Library side by side in a single library together with other library 152 | facilities that are not Applications and are not covered by this 153 | License, and convey such a combined library under terms of your 154 | choice, if you do both of the following: 155 | 156 | a) Accompany the combined library with a copy of the same work based 157 | on the Library, uncombined with any other library facilities, 158 | conveyed under the terms of this License. 159 | 160 | b) Give prominent notice with the combined library that part of it 161 | is a work based on the Library, and explaining where to find the 162 | accompanying uncombined form of the same work. 163 | 164 | 6. Revised Versions of the GNU Lesser General Public License. 165 | 166 | The Free Software Foundation may publish revised and/or new versions 167 | of the GNU Lesser General Public License from time to time. Such new 168 | versions will be similar in spirit to the present version, but may 169 | differ in detail to address new problems or concerns. 170 | 171 | Each version is given a distinguishing version number. If the 172 | Library as you received it specifies that a certain numbered version 173 | of the GNU Lesser General Public License "or any later version" 174 | applies to it, you have the option of following the terms and 175 | conditions either of that published version or of any later version 176 | published by the Free Software Foundation. If the Library as you 177 | received it does not specify a version number of the GNU Lesser 178 | General Public License, you may choose any version of the GNU Lesser 179 | General Public License ever published by the Free Software Foundation. 180 | 181 | If the Library as you received it specifies that a proxy can decide 182 | whether future versions of the GNU Lesser General Public License shall 183 | apply, that proxy's public statement of acceptance of any version is 184 | permanent authorization for you to choose that version for the 185 | Library. 186 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # SkiROS2 examples 2 | 3 | Collection of examples for [skiros2](https://github.com/RVMI/skiros2). 4 | 5 | ## Run 6 | 7 | ### Turtlesim example 8 | 9 | ```roslaunch skiros2_examples turtlesim_example.launch``` 10 | 11 | Run "turtle_spawn_and_follow" skill from skiros GUI 12 | 13 | ### Follow pose example 14 | 15 | ```roslaunch skiros2_examples follow_pose_example.launch``` 16 | 17 | Run "follow_pose" skill from skiros GUI 18 | 19 | ### Producer/Consumer example 20 | 21 | ```roslaunch skiros2_examples simple_params_example.launch``` 22 | 23 | Run "trajectory_coordinator" skill from skiros GUI 24 | 25 | ### Task planner example 26 | 27 | Note: you should have installed the Fast Downward planner using the script in `skiros2/skiros2/scripts` before running this example. 28 | 29 | ```roslaunch skiros2_examples planning_example.launch``` 30 | 31 | Run "task_plan" skill from skiros GUI with a Goal in PDDL format. For example: 32 | 33 | Place starter 145 in box 80: `(skiros:contain skiros:LargeBox-80 skiros:Starter-145)` 34 | 35 | Place all starters in box 80: `(forall (?x - skiros:Product) (skiros:contain skiros:LargeBox-51 ?x) )` 36 | 37 | -------------------------------------------------------------------------------- /config/config.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /TF1 10 | - /TF1/Frames1 11 | - /TF1/Tree1 12 | Splitter Ratio: 0.731457829 13 | Tree Height: 359 14 | - Class: rviz/Selection 15 | Name: Selection 16 | - Class: rviz/Tool Properties 17 | Expanded: 18 | - /2D Pose Estimate1 19 | - /2D Nav Goal1 20 | - /Publish Point1 21 | Name: Tool Properties 22 | Splitter Ratio: 0.588679016 23 | - Class: rviz/Views 24 | Expanded: 25 | - /Current View1 26 | Name: Views 27 | Splitter Ratio: 0.5 28 | - Class: rviz/Time 29 | Experimental: false 30 | Name: Time 31 | SyncMode: 0 32 | SyncSource: "" 33 | Visualization Manager: 34 | Class: "" 35 | Displays: 36 | - Alpha: 0.5 37 | Cell Size: 1 38 | Class: rviz/Grid 39 | Color: 160; 160; 164 40 | Enabled: true 41 | Line Style: 42 | Line Width: 0.0299999993 43 | Value: Lines 44 | Name: Grid 45 | Normal Cell Count: 0 46 | Offset: 47 | X: 0 48 | Y: 0 49 | Z: 0 50 | Plane: XY 51 | Plane Cell Count: 10 52 | Reference Frame: 53 | Value: true 54 | - Class: rviz/TF 55 | Enabled: true 56 | Frame Timeout: 15 57 | Frames: 58 | All Enabled: false 59 | cora:Robot-1: 60 | Value: true 61 | map: 62 | Value: false 63 | Marker Scale: 1 64 | Name: TF 65 | Show Arrows: true 66 | Show Axes: true 67 | Show Names: true 68 | Tree: 69 | map: 70 | cora:Robot-1: 71 | {} 72 | Update Interval: 0 73 | Value: true 74 | Enabled: true 75 | Global Options: 76 | Background Color: 48; 48; 48 77 | Default Light: true 78 | Fixed Frame: map 79 | Frame Rate: 30 80 | Name: root 81 | Tools: 82 | - Class: rviz/Interact 83 | Hide Inactive Objects: true 84 | - Class: rviz/MoveCamera 85 | - Class: rviz/Select 86 | - Class: rviz/FocusCamera 87 | - Class: rviz/Measure 88 | - Class: rviz/SetInitialPose 89 | Topic: /initialpose 90 | - Class: rviz/SetGoal 91 | Topic: /move_base_simple/goal 92 | - Class: rviz/PublishPoint 93 | Single click: true 94 | Topic: /clicked_point 95 | Value: true 96 | Views: 97 | Current: 98 | Class: rviz/Orbit 99 | Distance: 5.31108809 100 | Enable Stereo Rendering: 101 | Stereo Eye Separation: 0.0599999987 102 | Stereo Focal Distance: 1 103 | Swap Stereo Eyes: false 104 | Value: false 105 | Focal Point: 106 | X: 0 107 | Y: 0 108 | Z: 0 109 | Focal Shape Fixed Size: true 110 | Focal Shape Size: 0.0500000007 111 | Invert Z Axis: false 112 | Name: Current View 113 | Near Clip Distance: 0.00999999978 114 | Pitch: 0.650398552 115 | Target Frame: 116 | Value: Orbit (rviz) 117 | Yaw: 5.75360394 118 | Saved: ~ 119 | Window Geometry: 120 | Displays: 121 | collapsed: true 122 | Height: 1028 123 | Hide Left Dock: true 124 | Hide Right Dock: true 125 | QMainWindow State: 000000ff00000000fd0000000400000000000001890000037efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007300000000280000037e000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000037efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000037e000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003a00000003efc0100000002fb0000000800540069006d00650100000000000003a00000028200fffffffb0000000800540069006d00650100000000000004500000000000000000000003a00000037e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 126 | Selection: 127 | collapsed: false 128 | Time: 129 | collapsed: false 130 | Tool Properties: 131 | collapsed: false 132 | Views: 133 | collapsed: true 134 | Width: 928 135 | X: 1985 136 | Y: 24 137 | -------------------------------------------------------------------------------- /launch/action_example.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /launch/follow_pose_example.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /launch/main.launch: -------------------------------------------------------------------------------- 1 | 2 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /launch/planning_example.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /launch/simple_params_example.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /launch/turtlesim_example.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /owl/planning_scene.turtle: -------------------------------------------------------------------------------- 1 | @prefix SUMO: . 2 | @prefix cora: . 3 | @prefix coraX: . 4 | @prefix coraX1: . 5 | @prefix corax: . 6 | @prefix owl: . 7 | @prefix owl2xml: . 8 | @prefix rdf: . 9 | @prefix rdfs: . 10 | @prefix rparts: . 11 | @prefix skiros: . 12 | @prefix sumo: . 13 | @prefix tts: . 14 | @prefix xml: . 15 | @prefix xsd: . 16 | 17 | skiros:Drive a owl:class ; 18 | rdfs:subClassOf skiros:Skill . 19 | 20 | skiros:Locate a owl:class ; 21 | rdfs:subClassOf skiros:Skill . 22 | 23 | skiros:Pick a owl:class ; 24 | rdfs:subClassOf skiros:Skill . 25 | 26 | skiros:Place a owl:class ; 27 | rdfs:subClassOf skiros:Skill . 28 | 29 | skiros:Scene-0 a skiros:Scene, 30 | owl:NamedIndividual ; 31 | rdfs:label "" ; 32 | skiros:DiscreteReasoner "AauSpatialReasoner"^^xsd:string ; 33 | skiros:FrameId "map"^^xsd:string ; 34 | skiros:OrientationW "1.0"^^xsd:float ; 35 | skiros:OrientationX "0.0"^^xsd:float ; 36 | skiros:OrientationY "0.0"^^xsd:float ; 37 | skiros:OrientationZ "0.0"^^xsd:float ; 38 | skiros:PositionX "0.0"^^xsd:float ; 39 | skiros:PositionY "0.0"^^xsd:float ; 40 | skiros:PositionZ "0.0"^^xsd:float ; 41 | skiros:contain skiros:LargeBox-51, 42 | skiros:LargeBox-80. 43 | 44 | skiros:TaskPlan a owl:class ; 45 | rdfs:subClassOf skiros:Skill . 46 | 47 | skiros:LargeBox-51 a skiros:LargeBox, 48 | owl:NamedIndividual ; 49 | rdfs:label "start_box" ; 50 | skiros:BaseFrameId ""^^xsd:string ; 51 | skiros:DiscreteReasoner "AauSpatialReasoner"^^xsd:string ; 52 | skiros:FrameId "skiros:LargeBox-51"^^xsd:string ; 53 | skiros:PublishTf true ; 54 | skiros:Template "skiros:large_box_test_starter"^^xsd:string ; 55 | skiros:contain skiros:Starter-145, 56 | skiros:Starter-52 ; 57 | skiros:hasTemplate skiros:large_box_test_starter . 58 | 59 | skiros:LargeBox-80 a skiros:LargeBox, 60 | owl:NamedIndividual ; 61 | rdfs:label "target_box" ; 62 | skiros:BaseFrameId ""^^xsd:string ; 63 | skiros:DiscreteReasoner "AauSpatialReasoner"^^xsd:string ; 64 | skiros:FrameId "skiros:LargeBox-80"^^xsd:string ; 65 | skiros:PublishTf true ; 66 | skiros:Template "skiros:large_box_test_starter"^^xsd:string ; 67 | skiros:hasTemplate skiros:large_box_test_starter . 68 | 69 | skiros:Starter-145 a skiros:Starter, 70 | owl:NamedIndividual ; 71 | rdfs:label "starter2" ; 72 | skiros:BaseFrameId ""^^xsd:string ; 73 | skiros:DiscreteReasoner "AauSpatialReasoner"^^xsd:string ; 74 | skiros:FrameId "skiros:Starter-145"^^xsd:string ; 75 | skiros:PublishTf true ; 76 | skiros:Template "skiros:starter"^^xsd:string ; 77 | skiros:Weight "3.0"^^xsd:float ; 78 | skiros:hasTemplate skiros:starter . 79 | 80 | skiros:Starter-52 a skiros:Starter, 81 | owl:NamedIndividual ; 82 | rdfs:label "starter1" ; 83 | skiros:BaseFrameId ""^^xsd:string ; 84 | skiros:DiscreteReasoner "AauSpatialReasoner"^^xsd:string ; 85 | skiros:FrameId "skiros:Starter-52"^^xsd:string ; 86 | skiros:PublishTf true ; 87 | skiros:Template "skiros:starter"^^xsd:string ; 88 | skiros:Weight "3.0"^^xsd:float ; 89 | skiros:hasTemplate skiros:starter . 90 | 91 | -------------------------------------------------------------------------------- /owl/turtlesim_robot_description.owl: -------------------------------------------------------------------------------- 1 | 2 | 11 | 12 | 13 | 14 | 15 | 16 | 23 | 24 | 25 | 26 | 27 | 28 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | AauSpatialReasoner 42 | /turtlebot_robot 43 | /turtle1 44 | AauSpatialReasoner 45 | 1 46 | 0 47 | 0 48 | 0 49 | 0 50 | 0 51 | 0 52 | 53 | 54 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | skiros2_examples 4 | 1.0.0 5 | Set of examples for the skiros2 platform 6 | 7 | Francesco Rovida 8 | Bjarne Grossmann 9 | L-GPL 10 | Francesco Rovida 11 | Francesco Rovida 12 | 13 | 14 | catkin 15 | rospy 16 | skiros2_msgs 17 | rospy 18 | skiros2_msgs 19 | skiros2_common 20 | skiros2_skill 21 | turtlesim 22 | 23 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | ## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD 2 | 3 | from distutils.core import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | # fetch values from package.xml 7 | setup_args = generate_distutils_setup( 8 | packages=['skiros2_examples'], 9 | package_dir={'': 'src'}, 10 | ) 11 | 12 | setup(**setup_args) 13 | -------------------------------------------------------------------------------- /src/skiros2_examples/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobotLabLTH/skiros2_examples/bd157c9b4888127784711f92bf47b42896608103/src/skiros2_examples/__init__.py -------------------------------------------------------------------------------- /src/skiros2_examples/action_example/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobotLabLTH/skiros2_examples/bd157c9b4888127784711f92bf47b42896608103/src/skiros2_examples/action_example/__init__.py -------------------------------------------------------------------------------- /src/skiros2_examples/action_example/skill_client.py: -------------------------------------------------------------------------------- 1 | from skiros2_skill.core.skill import SkillDescription 2 | from skiros2_common.core.params import ParamTypes 3 | from skiros2_common.core.primitive import PrimitiveBase 4 | from skiros2_std_skills.action_client_primitive import PrimitiveActionClient 5 | import skiros2_common.tools.logger as log 6 | import actionlib 7 | from actionlib.msg import TestAction, TestGoal 8 | 9 | class ActionSkillDescription(SkillDescription): 10 | def createDescription(self): 11 | #=======Params========= 12 | self.addParam("InputParam", 0, ParamTypes.Required) 13 | self.addParam("OutputParam", 0, ParamTypes.Optional) 14 | #=======PreConditions========= 15 | 16 | class DriveDescription(SkillDescription): 17 | def createDescription(self): 18 | #=======Params========= 19 | self.addParam("Mode", 'DIFFERENTIAL', ParamTypes.Required) 20 | self.addParam("Target", '', ParamTypes.Required) 21 | #=======PreConditions========= 22 | 23 | class test_action_skill(PrimitiveActionClient): 24 | """ 25 | @brief A skill that connects to a test action server 26 | 27 | Goal and feeback is just an integer 28 | """ 29 | def createDescription(self): 30 | self.setDescription(ActionSkillDescription(), self.__class__.__name__) 31 | 32 | def buildClient(self): 33 | return actionlib.SimpleActionClient('/test_action_server', TestAction) 34 | 35 | def buildGoal(self): 36 | return TestGoal(self.params["InputParam"].value) 37 | 38 | def onFeedback(self, msg): 39 | self.params["OutputParam"].value = msg.feedback 40 | return self.step("{}".format(self.params["OutputParam"].value)) 41 | 42 | -------------------------------------------------------------------------------- /src/skiros2_examples/action_example/test_action_server: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | import rospy 3 | import actionlib 4 | from actionlib.msg import TestAction, TestFeedback, TestResult 5 | 6 | class ExampleActionServer(object): 7 | """ 8 | Just count to 20 starting from the goal integer 9 | """ 10 | def __init__(self): 11 | # Init server 12 | rospy.init_node('test_action_server') 13 | self.server = actionlib.SimpleActionServer('test_action_server', TestAction, execute_cb = self.execute, auto_start = False) 14 | self.rate = rospy.Rate(10.0) 15 | self.feedback = TestFeedback() 16 | self.result = TestResult() 17 | self.server.start() 18 | 19 | def execute(self, goal): 20 | self.state = 'initializing' 21 | rospy.loginfo('state: ' + self.state) 22 | self.counter = goal.goal 23 | while (not self.server.is_preempt_requested()) and (not rospy.is_shutdown()): 24 | self.counter += 1 25 | self.feedback.feedback = self.counter 26 | self.server.publish_feedback(self.feedback) 27 | self.rate.sleep() 28 | if self.counter > 20: 29 | self.result.result = self.counter 30 | self.server.set_succeeded(self.result) 31 | self.state = 'inactive' 32 | rospy.loginfo('state: ' + self.state) 33 | return 34 | 35 | self.server.set_preempted() 36 | self.state = 'inactive' 37 | rospy.loginfo('state: ' + self.state) 38 | 39 | if __name__ == '__main__': 40 | server = ExampleActionServer() 41 | rospy.spin() 42 | -------------------------------------------------------------------------------- /src/skiros2_examples/follow_pose_example/__init__.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | -------------------------------------------------------------------------------- /src/skiros2_examples/follow_pose_example/follow_pose_primitives.py: -------------------------------------------------------------------------------- 1 | from skiros2_skill.core.skill import SkillDescription 2 | from skiros2_common.core.params import ParamTypes 3 | from skiros2_common.core.world_element import Element 4 | from skiros2_common.core.primitive import PrimitiveBase 5 | 6 | import numpy as np 7 | 8 | ################################################################################# 9 | # Descriptions 10 | ################################################################################# 11 | 12 | 13 | class PoseGenerator(SkillDescription): 14 | def createDescription(self): 15 | # =======Params========= 16 | self.addParam("Pose", Element("skiros:TransformationPose"), ParamTypes.Optional) 17 | self.addParam("x", float, ParamTypes.Required) 18 | self.addParam("y", float, ParamTypes.Required) 19 | self.addParam("z", float, ParamTypes.Required) 20 | 21 | 22 | class PoseMover(SkillDescription): 23 | def createDescription(self): 24 | # =======Params========= 25 | self.addParam("Pose", Element("skiros:TransformationPose"), ParamTypes.Required) 26 | self.addParam("Direction", 0, ParamTypes.Required, description="x: 0, y: 1, z: 2") 27 | 28 | 29 | class PoseFollowerOneAxis(SkillDescription): 30 | def createDescription(self): 31 | # =======Params========= 32 | self.addParam("Pose", Element("skiros:TransformationPose"), ParamTypes.Required) 33 | self.addParam("Pose2", Element("skiros:TransformationPose"), ParamTypes.Required) 34 | self.addParam("Axis", float, ParamTypes.Required) 35 | 36 | 37 | class PoseFollowerTwoAxis(SkillDescription): 38 | def createDescription(self): 39 | # =======Params========= 40 | self.addParam("Pose", Element("skiros:TransformationPose"), ParamTypes.Required) 41 | self.addParam("Pose2", Element("skiros:TransformationPose"), ParamTypes.Required) 42 | self.addParam("Axis1", float, ParamTypes.Required) 43 | self.addParam("Axis2", float, ParamTypes.Required) 44 | 45 | 46 | class PoseFollowerThreeAxis(SkillDescription): 47 | def createDescription(self): 48 | # =======Params========= 49 | self.addParam("Pose", Element("skiros:TransformationPose"), ParamTypes.Required) 50 | self.addParam("Pose2", Element("skiros:TransformationPose"), ParamTypes.Required) 51 | ################################################################################# 52 | # Implementations 53 | ################################################################################# 54 | 55 | 56 | class pose_generator(PrimitiveBase): 57 | """ 58 | Creates a TransformationPose on the world model, if not already present 59 | """ 60 | 61 | def createDescription(self): 62 | self.setDescription(PoseGenerator(), self.__class__.__name__) 63 | 64 | def execute(self): 65 | pose = self.params["Pose"].value 66 | if pose.id == "": 67 | pose.setData(":Position", [self.params["x"].value, self.params["y"].value, self.params["z"].value]) 68 | pose.setData(":Orientation", [0.0, 0.0, 0.0, 1.0]) 69 | pose.addRelation("skiros:Scene-0", "skiros:contain", "-1") 70 | self.wmi.add_element(pose) 71 | return self.success("Done") 72 | 73 | 74 | class linear_mover(PrimitiveBase): 75 | """ 76 | This primitive has 1 state when progress is < 10 and 1 state of success 77 | """ 78 | 79 | def createDescription(self): 80 | self.setDescription(PoseMover(), self.__class__.__name__) 81 | 82 | def execute(self): 83 | pose = self.params["Pose"].value 84 | direction = self._params.getParamValue("Direction") 85 | position = pose.getData(":Position") 86 | position[direction] = position[direction] + 0.1 87 | pose.setData(":Position", position) 88 | self.params["Pose"].value = pose 89 | if self._progress_code < 10: 90 | return self.step("Changing position to: {}".format(position)) 91 | else: 92 | return self.success("Done") 93 | 94 | 95 | class angular_mover(PrimitiveBase): 96 | """ 97 | This primitive has 1 state when progress is < 10 and 1 state of success 98 | """ 99 | 100 | def createDescription(self): 101 | self.setDescription(PoseMover(), self.__class__.__name__) 102 | 103 | def onPreempt(self): 104 | return self.success("Done") 105 | 106 | def execute(self): 107 | pose = self.params["Pose"].value 108 | o = pose.getData(":OrientationEuler") 109 | d = self._params.getParamValue("Direction") 110 | o[d] = o[d] + 0.1 111 | pose.setData(":OrientationEuler", o) 112 | self.wmi.update_element_properties(pose, "AauSpatialReasoner") 113 | if self._progress_code < 10: 114 | return self.step("Changing orientation to: {}".format(o)) 115 | else: 116 | return self.success("Done") 117 | 118 | 119 | class rotation_mover(PrimitiveBase): 120 | """ 121 | """ 122 | 123 | def createDescription(self): 124 | self.setDescription(PoseMover(), self.__class__.__name__) 125 | 126 | def onPreempt(self): 127 | return self.success("Done") 128 | 129 | def execute(self): 130 | pose = self.params["Pose"].value 131 | o = pose.getData(":OrientationEuler") 132 | d = self._params.getParamValue("Direction") 133 | o[d] = o[d] + 0.1 134 | pose.setData(":OrientationEuler", o) 135 | self.wmi.update_element_properties(o, "AauSpatialReasoner") 136 | return self.step("Changing orientation to: {}".format(o)) 137 | 138 | 139 | class pose_follower_one_axis(PrimitiveBase): 140 | """ 141 | This primitive makes a pose follow another one along one given axis 142 | It stops when is close to the target 143 | """ 144 | 145 | dist = 0 146 | proximityThreshold = 0.05 147 | 148 | def createDescription(self): 149 | self.setDescription(PoseFollowerOneAxis(), self.__class__.__name__) 150 | 151 | def onPreempt(self): 152 | return self.success("Done") 153 | 154 | def execute(self): 155 | pose = self._params.getParamValue("Pose") 156 | pose2 = self._params.getParamValue("Pose2") 157 | position = pose.getData(":Position") 158 | position2 = pose2.getData(":Position") 159 | self.dist = 0 160 | axis = int(self._params.getParamValue("Axis")) 161 | diff = position2[axis] - position[axis] 162 | self.dist += diff**2 163 | if diff != 0: 164 | diff = diff / 4 165 | position2[axis] -= diff 166 | pose2.setData(":Position", position2) 167 | self.params["Pose2"].value = pose2 168 | 169 | if self.dist >= self.proximityThreshold: 170 | return self.step("Following pose: {}".format(position)) 171 | else: 172 | return self.success("Successful following along axis: {}".format(axis)) 173 | 174 | 175 | class pose_follower_two_axis(PrimitiveBase): 176 | """ 177 | This primitive makes a pose follow another one along two given axis 178 | It stops when is close to the target 179 | """ 180 | 181 | dist = 0 182 | proximityThreshold = 0.05 183 | 184 | def createDescription(self): 185 | self.setDescription(PoseFollowerTwoAxis(), self.__class__.__name__) 186 | 187 | def onPreempt(self): 188 | return self.success("Done") 189 | 190 | def execute(self): 191 | pose = self._params.getParamValue("Pose") 192 | pose2 = self._params.getParamValue("Pose2") 193 | position = pose.getData(":Position") 194 | position2 = pose2.getData(":Position") 195 | self.dist = 0 196 | axis = [int(self._params.getParamValue("Axis1")), int(self._params.getParamValue("Axis2"))] 197 | for i in axis: 198 | diff = position2[i] - position[i] 199 | self.dist += diff**2 200 | if diff != 0: 201 | diff = diff / 4 202 | position2[i] -= diff 203 | pose2.setData(":Position", position2) 204 | self.params["Pose2"].value = pose2 205 | 206 | if self.dist >= self.proximityThreshold: 207 | return self.step("Following pose: {}".format(position)) 208 | else: 209 | return self.success("Successful following along axis: {}".format(axis)) 210 | 211 | 212 | class pose_follower_three_axis(PrimitiveBase): 213 | """ 214 | This primitive stops when is close to the target 215 | """ 216 | dist = 0 217 | proximityThreshold = 0.05 218 | 219 | def createDescription(self): 220 | self.setDescription(PoseFollowerThreeAxis(), self.__class__.__name__) 221 | 222 | def onPreempt(self): 223 | return self.success("Done") 224 | 225 | def execute(self): 226 | pose = self._params.getParamValue("Pose") 227 | pose2 = self._params.getParamValue("Pose2") 228 | position = pose.getData(":Position") 229 | position2 = pose2.getData(":Position") 230 | self.dist = 0 231 | for i in range(0, 3): 232 | diff = position2[i] - position[i] 233 | self.dist += diff**2 234 | if diff != 0: 235 | diff = diff / 4 236 | position2[i] -= diff 237 | pose2.setData(":Position", position2) 238 | self.params["Pose2"].value = pose2 239 | 240 | if self.dist >= self.proximityThreshold: 241 | return self.step("Following pose: {}".format(position)) 242 | else: 243 | return self.success("Successful following") 244 | 245 | 246 | class pose_circle_mover(PrimitiveBase): 247 | """ 248 | Makes the selected pose turn around the selected axis 249 | This primitive doesn't stop until it is preempted explicitely 250 | """ 251 | angle = 0.05 252 | 253 | def createDescription(self): 254 | self.setDescription(PoseMover(), self.__class__.__name__) 255 | 256 | def onPreempt(self): 257 | return self.success("Done") 258 | 259 | def execute(self): 260 | pose = self.params["Pose"].value 261 | p = pose.getData(":Position") 262 | o = pose.getData(":OrientationEuler") 263 | d = self._params.getParamValue("Direction") 264 | rotationMatrix = np.array([[np.cos(self.angle), np.sin(self.angle)], [-np.sin(self.angle), np.cos(self.angle)]]) 265 | if d == 2: 266 | # in that case we turn around the z axis 267 | xyRotated = np.dot(rotationMatrix, np.array([p[0], p[1]])) 268 | p[0] = xyRotated[0] 269 | p[1] = xyRotated[1] 270 | elif d == 1: 271 | # in that case we turn around the y axis 272 | xzRotated = np.dot(rotationMatrix, np.array([p[0], p[2]])) 273 | p[0] = xzRotated[0] 274 | p[2] = xzRotated[1] 275 | elif d == 0: 276 | # in that case we turn around the x axis 277 | yzRotated = np.dot(rotationMatrix, np.array([p[1], p[2]])) 278 | p[1] = yzRotated[0] 279 | p[2] = yzRotated[1] 280 | else: 281 | return self.fail("wrong direction", -1) 282 | 283 | o[d] = o[d] - self.angle 284 | pose.setData(":OrientationEuler", o) 285 | pose.setData(":Position", p) 286 | self.wmi.update_element_properties(pose, "AauSpatialReasoner") 287 | return self.step("turning") 288 | -------------------------------------------------------------------------------- /src/skiros2_examples/follow_pose_example/follow_pose_skills.py: -------------------------------------------------------------------------------- 1 | from skiros2_skill.core.skill import SkillDescription, SkillBase, ParallelFs, SerialStar, Serial, ParallelFf 2 | from skiros2_common.core.params import ParamTypes 3 | from skiros2_common.core.world_element import Element 4 | 5 | 6 | ################################################################################# 7 | # Description 8 | ################################################################################# 9 | 10 | class FollowPose(SkillDescription): 11 | def createDescription(self): 12 | # =======Params========= 13 | self.addParam("Pose", Element("skiros:TransformationPose"), ParamTypes.Optional) 14 | self.addParam("Pose2", Element("skiros:TransformationPose"), ParamTypes.Optional) 15 | self.addParam("Pose3", Element("skiros:TransformationPose"), ParamTypes.Optional) 16 | 17 | ################################################################################# 18 | # Implementation 19 | ################################################################################# 20 | 21 | 22 | class follow_pose(SkillBase): 23 | def createDescription(self): 24 | self.setDescription(FollowPose(), self.__class__.__name__) 25 | 26 | def expand(self, skill): 27 | skill.setProcessor(SerialStar()) 28 | 29 | skill( 30 | self.skill(SerialStar())( 31 | self.skill("PoseGenerator", "", 32 | specify={"x": 1., "y": 0., "z": 0.}), 33 | self.skill("PoseGenerator", "", 34 | specify={"x": 1., "y": 1., "z": 1.}, remap={"Pose": "Pose2"}), 35 | self.skill("PoseGenerator", "", 36 | specify={"x": 0., "y": 2., "z": 2.}, remap={"Pose": "Pose3"}), 37 | ), 38 | self.skill(ParallelFs())( 39 | self.skill("PoseMover", "pose_circle_mover", specify={"Direction": 2}), 40 | self.skill(SerialStar())( 41 | self.skill("PoseFollowerTwoAxis", "pose_follower_two_axis", 42 | specify={"Axis1": 0., "Axis2": 1.}), 43 | self.skill("PoseFollowerThreeAxis", "pose_follower_three_axis"), 44 | self.skill("PoseFollowerOneAxis", "pose_follower_one_axis", 45 | specify={"Axis": 2.}, remap={"Pose": "Pose3"}), 46 | self.skill("PoseFollowerThreeAxis", "pose_follower_three_axis", 47 | remap={"Pose": "Pose3"}) 48 | ) 49 | 50 | ) 51 | ) 52 | -------------------------------------------------------------------------------- /src/skiros2_examples/planning_example/__init__.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | -------------------------------------------------------------------------------- /src/skiros2_examples/planning_example/planning_skills.py: -------------------------------------------------------------------------------- 1 | from skiros2_skill.core.skill import SkillDescription, SkillBase, ParamOptions 2 | from skiros2_common.core.params import ParamTypes 3 | from skiros2_common.core.world_element import Element 4 | 5 | ################################################################################# 6 | # Description 7 | ################################################################################# 8 | 9 | class Locate(SkillDescription): 10 | def createDescription(self): 11 | #=======Params========= 12 | self.addParam("Container", Element("skiros:Location"), ParamTypes.Required) 13 | self.addParam("Object", Element("skiros:Product"), ParamTypes.Optional) 14 | self.addParam("Camera", Element("skiros:DepthCamera"), ParamTypes.Required, [ParamOptions.Lock]) 15 | #=======PreConditions========= 16 | self.addPreCondition(self.getRelationCond("RobotAt", "skiros:at", "Robot", "Container", True)) 17 | self.addPreCondition(self.getAbstractRelationCond("ContainerForObject", "skiros:partReference", "Container", "Object", True)) 18 | #=======PostConditions========= 19 | self.addPostCondition(self.getRelationCond("InContainer", "skiros:contain", "Container", "Object", True)) 20 | self.addPostCondition(self.getHasPropCond("HasPosition", "skiros:Position", "Object", True)) 21 | 22 | class Drive(SkillDescription): 23 | def createDescription(self): 24 | #=======Params========= 25 | self.addParam("StartLocation", Element("skiros:Location"), ParamTypes.Inferred) 26 | self.addParam("TargetLocation", Element("skiros:Location"), ParamTypes.Required) 27 | #=======PreConditions========= 28 | self.addPreCondition(self.getRelationCond("RobotAt", "skiros:at", "Robot", "StartLocation", True)) 29 | #=======PostConditions========= 30 | self.addPostCondition(self.getRelationCond("NoRobotAt", "skiros:at", "Robot", "StartLocation", False)) 31 | self.addPostCondition(self.getRelationCond("RobotAt", "skiros:at", "Robot", "TargetLocation", True)) 32 | 33 | class Pick(SkillDescription): 34 | def createDescription(self): 35 | #=======Params========= 36 | self.addParam("Container", Element("skiros:Location"), ParamTypes.Inferred) 37 | self.addParam("Object", Element("skiros:Product"), ParamTypes.Required) 38 | self.addParam("Arm", Element("rparts:ArmDevice"), ParamTypes.Required) 39 | self.addParam("Gripper", Element("rparts:GripperEffector"), ParamTypes.Inferred) 40 | #=======PreConditions========= 41 | self.addPreCondition(self.getPropCond("EmptyHanded", "skiros:ContainerState", "Gripper", "=", "Empty", True)) 42 | self.addPreCondition(self.getRelationCond("ObjectInContainer", "skiros:contain", "Container", "Object", True)) 43 | #=======HoldConditions========= 44 | self.addHoldCondition(self.getRelationCond("RobotAtLocation", "skiros:at", "Robot", "Container", True)) 45 | #=======PostConditions========= 46 | self.addPostCondition(self.getPropCond("EmptyHanded", "skiros:ContainerState", "Gripper", "=", "Empty", False)) 47 | self.addPostCondition(self.getRelationCond("Holding", "skiros:contain", "Gripper", "Object", True)) 48 | 49 | class Place(SkillDescription): 50 | def createDescription(self): 51 | #=======Params========= 52 | self.addParam("PlacingLocation", Element("skiros:Location"), ParamTypes.Required) 53 | self.addParam("Arm", Element("rparts:ArmDevice"), ParamTypes.Required) 54 | self.addParam("Gripper", Element("rparts:GripperEffector"), ParamTypes.Inferred) 55 | self.addParam("Object", Element("skiros:Product"), ParamTypes.Inferred) 56 | #=======PreConditions========= 57 | self.addPreCondition(self.getRelationCond("Holding", "skiros:contain", "Gripper", "Object", True)) 58 | #=======HoldConditions========= 59 | self.addHoldCondition(self.getRelationCond("RobotAtLocation", "skiros:at", "Robot", "PlacingLocation", True)) 60 | #=======PostConditions========= 61 | self.addPostCondition(self.getPropCond("EmptyHanded", "skiros:ContainerState", "Gripper", "=", "Empty", True)) 62 | self.addPostCondition(self.getRelationCond("NotHolding", "skiros:contain", "Gripper", "Object", False)) 63 | self.addPostCondition(self.getRelationCond("InPlace", "skiros:contain", "PlacingLocation", "Object", True)) 64 | 65 | ################################################################################# 66 | # Implementation 67 | ################################################################################# 68 | 69 | 70 | class locate_fake(SkillBase): 71 | def createDescription(self): 72 | self.setDescription(Locate(), self.__class__.__name__) 73 | 74 | def expand(self, skill): 75 | skill( 76 | self.skill("Wait", "wait", specify={"Duration": 1.0}), 77 | ) 78 | 79 | class drive_fake(SkillBase): 80 | def createDescription(self): 81 | self.setDescription(Drive(), self.__class__.__name__) 82 | 83 | def set_at(self, src, dst, state): 84 | return self.skill("WmSetRelation", "wm_set_relation", 85 | remap={'Dst': dst}, 86 | specify={'Src': self.params[src].value, 'Relation': 'skiros:at', 'RelationState': state}) 87 | 88 | def expand(self, skill): 89 | skill( 90 | self.skill("Wait", "wait", specify={"Duration": 1.0}), 91 | self.set_at("Robot", "StartLocation", False), 92 | self.set_at("Robot", "TargetLocation", True), 93 | ) 94 | 95 | class pick_fake(SkillBase): 96 | def createDescription(self): 97 | self.setDescription(Pick(), self.__class__.__name__) 98 | 99 | def expand(self, skill): 100 | skill( 101 | self.skill("Wait", "wait", specify={"Duration": 1.0}), 102 | self.skill("WmMoveObject", "wm_move_object", 103 | remap={"StartLocation": "Container", "TargetLocation": "Gripper"}), 104 | ) 105 | 106 | class place_fake(SkillBase): 107 | def createDescription(self): 108 | self.setDescription(Place(), self.__class__.__name__) 109 | 110 | def expand(self, skill): 111 | skill( 112 | self.skill("Wait", "wait", specify={"Duration": 1.0}), 113 | self.skill("WmMoveObject", "wm_move_object", 114 | remap={"StartLocation": "Gripper", "TargetLocation": "PlacingLocation"}), 115 | ) 116 | 117 | 118 | -------------------------------------------------------------------------------- /src/skiros2_examples/simple_params_example/__init__.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | -------------------------------------------------------------------------------- /src/skiros2_examples/simple_params_example/simple_params_primitives.py: -------------------------------------------------------------------------------- 1 | from skiros2_skill.core.skill import SkillDescription, ParamOptions 2 | from skiros2_common.core.params import ParamTypes 3 | from skiros2_common.core.world_element import Element 4 | from skiros2_common.core.primitive import PrimitiveBase 5 | import threading 6 | try: 7 | import Queue as queue 8 | except ImportError: 9 | import queue 10 | 11 | ################################################################################# 12 | # Descriptions 13 | ################################################################################# 14 | 15 | class TrajectoryGenerator(SkillDescription): 16 | def createDescription(self): 17 | #=======Params========= 18 | self.addParam("Trajectory", dict, ParamTypes.Optional) 19 | self.addParam("Shutdown", False, ParamTypes.Optional) 20 | 21 | class TrajectoryConsumer(SkillDescription): 22 | def createDescription(self): 23 | #=======Params========= 24 | self.addParam("Trajectory", dict, ParamTypes.Required) 25 | self.addParam("Shutdown", bool, ParamTypes.Required) 26 | 27 | ################################################################################# 28 | # Implementations 29 | ################################################################################# 30 | 31 | import random 32 | import time 33 | 34 | class trajectory_generator(PrimitiveBase): 35 | """ 36 | This primitive generates fake "trajectories", under form of dictionaries 37 | 38 | To support a planning process that could take a long time, the plan function is executed in a parallel thread 39 | and outputs a plan every second. The execute functions just updates the parameters. 40 | 41 | Since parameters can be updated ONLY in the EXECUTE function, a syncronized queue is necessary 42 | """ 43 | q = queue.Queue(1) 44 | is_done = False 45 | iterations = 2 46 | 47 | def onStart(self): 48 | self.is_done = False 49 | self.worker = threading.Thread(target=self.plan) 50 | self.worker.start() 51 | return True 52 | 53 | def createDescription(self): 54 | self.setDescription(TrajectoryGenerator(), self.__class__.__name__) 55 | 56 | def plan(self): 57 | for i in range(0, self.iterations): 58 | self.q.put([random.random() for _ in xrange(5)]) 59 | time.sleep(1) 60 | self.is_done = True 61 | 62 | def execute(self): 63 | if self.is_done: 64 | self.params["Shutdown"].setValue(True) 65 | return self.success("Done") 66 | if not self.q.empty(): 67 | traj = {} 68 | traj["Trajectory"] = self.q.get() 69 | self.params["Trajectory"].addValue(traj) 70 | return self.step("Added trajectory: {}".format(self.params["Trajectory"].getValues())) 71 | return self.step("") 72 | 73 | class trajectory_consumer(PrimitiveBase): 74 | """ 75 | This primitive consumes one value in "trajectories" at each tick 76 | 77 | It continues to run until the variable Shutdown is set to True 78 | """ 79 | def createDescription(self): 80 | self.setDescription(TrajectoryConsumer(), self.__class__.__name__) 81 | 82 | def execute(self): 83 | if self.params["Trajectory"].isSpecified(): 84 | trajs = self.params["Trajectory"].getValues() 85 | traj = trajs.pop(0) 86 | return self.step("Consumed trajectory: {}".format(traj)) 87 | if self.params["Shutdown"].value: 88 | return self.success("Done") 89 | return self.step("") 90 | -------------------------------------------------------------------------------- /src/skiros2_examples/simple_params_example/simple_params_skills.py: -------------------------------------------------------------------------------- 1 | from skiros2_skill.core.skill import SkillDescription, SkillBase, ParallelFf 2 | from skiros2_common.core.params import ParamTypes 3 | from skiros2_common.core.world_element import Element 4 | 5 | 6 | ################################################################################# 7 | # Description 8 | ################################################################################# 9 | 10 | class TrajectoryCoordinator(SkillDescription): 11 | def createDescription(self): 12 | #=======Params========= 13 | #self.addParam("Container", Element(":Location"), ParamTypes.World) 14 | #self.addParam("Object", Element(":Product"), ParamTypes.Optional) 15 | self.addParam("Pose", Element("skiros:TransformationPose"), ParamTypes.Optional) 16 | self.addParam("Pose2", Element("skiros:TransformationPose"), ParamTypes.Optional) 17 | 18 | ################################################################################# 19 | # Implementation 20 | ################################################################################# 21 | 22 | class trajectory_coordinator(SkillBase): 23 | def createDescription(self): 24 | self.setDescription(TrajectoryCoordinator(), self.__class__.__name__) 25 | 26 | def expand(self, skill): 27 | skill.setProcessor(ParallelFf()) 28 | skill( 29 | self.skill("TrajectoryGenerator", ""), 30 | self.skill("TrajectoryConsumer", "") 31 | ) 32 | 33 | -------------------------------------------------------------------------------- /src/skiros2_examples/turtlesim_example/__init__.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | -------------------------------------------------------------------------------- /src/skiros2_examples/turtlesim_example/turtlesim_primitives.py: -------------------------------------------------------------------------------- 1 | from skiros2_skill.core.skill import SkillDescription, ParamOptions 2 | from skiros2_common.core.params import ParamTypes 3 | from skiros2_common.core.world_element import Element 4 | from skiros2_common.core.primitive import PrimitiveBase 5 | import rospy 6 | import turtlesim.msg as ts 7 | from geometry_msgs.msg import Twist 8 | from turtlesim.srv import Spawn as SpawnSrv 9 | import threading, numpy 10 | 11 | import numpy as np 12 | import math 13 | 14 | 15 | ################################################################################# 16 | # Spawn 17 | ################################################################################# 18 | 19 | class Spawn(SkillDescription): 20 | def createDescription(self): 21 | #=======Params========= 22 | self.addParam("Name", str, ParamTypes.Required) 23 | self.addParam("X", 0.0, ParamTypes.Required) 24 | self.addParam("Y", 0.0, ParamTypes.Required) 25 | self.addParam("Rotation", 0.0, ParamTypes.Required) 26 | self.addParam("Turtle", Element("cora:Robot"), ParamTypes.Optional) 27 | 28 | class spawn(PrimitiveBase): 29 | def createDescription(self): 30 | self.setDescription(Spawn(), self.__class__.__name__) 31 | 32 | def execute(self): 33 | turtle = self.params["Turtle"].value 34 | name = self.params["Name"].value 35 | if turtle.id == "": 36 | try: 37 | spawner = rospy.ServiceProxy('spawn', SpawnSrv) 38 | resp = spawner(self.params["X"].value , self.params["Y"].value, math.radians(self.params["Rotation"].value), name) 39 | except rospy.ServiceException as e: 40 | return self.fail("Spawning turtle failed.", -1) 41 | 42 | turtle.label = "turtlebot:" + name 43 | turtle.setProperty("turtlebot:TurtleName", "/{}".format(name)) 44 | turtle.setData(":Position", [self.params["X"].value, self.params["Y"].value, 0.0]) 45 | turtle.setData(":OrientationEuler", [0.0, 0.0, math.radians(self.params["Rotation"].value)]) 46 | turtle.addRelation("skiros:Scene-0", "skiros:contain", "-1") 47 | turtle = self._wmi.add_element(turtle) 48 | self.params["Turtle"].value = turtle 49 | 50 | return self.success("Spawned turtle {}".format(name)) 51 | return self.success("") 52 | 53 | 54 | ################################################################################# 55 | # Command 56 | ################################################################################# 57 | 58 | class Command(SkillDescription): 59 | def createDescription(self): 60 | self.addParam("Turtle", Element("cora:Robot"), ParamTypes.Required) 61 | self.addParam("Linear", float, ParamTypes.Required, "Linear velocity") 62 | self.addParam("Angular", float, ParamTypes.Required, "Angular velocity in degrees") 63 | 64 | class command(PrimitiveBase): 65 | def createDescription(self): 66 | self.setDescription(Command(), self.__class__.__name__) 67 | 68 | def _send_command(self, linear, angular): 69 | msg = Twist() 70 | msg.linear.x = linear 71 | msg.angular.z = math.radians(angular) 72 | self.pose_pub.publish(msg) 73 | 74 | def onPreempt(self): 75 | return self.success("Preempted") 76 | 77 | def onEnd(self): 78 | self._send_command(0,0) 79 | return True 80 | 81 | def onStart(self): 82 | turtle = self.params["Turtle"].value.getProperty("turtlebot:TurtleName").value 83 | self.pose_pub = rospy.Publisher(turtle + "/cmd_vel", Twist, queue_size=20) 84 | return True 85 | 86 | def execute(self): 87 | turtle = self.params["Turtle"].value.getProperty("turtlebot:TurtleName").value 88 | self._send_command(self.params["Linear"].value, self.params["Angular"].value) 89 | return self.step("{}: moving at [{} {}]".format(turtle, self.params["Linear"].value, self.params["Angular"].value)) 90 | 91 | 92 | ################################################################################# 93 | # Monitor 94 | ################################################################################# 95 | 96 | class Monitor(SkillDescription): 97 | def createDescription(self): 98 | #=======Params========= 99 | self.addParam("Turtle", Element("cora:Robot"), ParamTypes.Required) 100 | 101 | class monitor(PrimitiveBase): 102 | def createDescription(self): 103 | self.setDescription(Monitor(), self.__class__.__name__) 104 | 105 | def onPreempt(self): 106 | return self.success("Preempted.") 107 | 108 | def onStart(self): 109 | name = self.params["Turtle"].value.getProperty("turtlebot:TurtleName").value 110 | self._pose_sub = rospy.Subscriber(name + "/pose", ts.Pose, self._monitor) 111 | self._pose = None 112 | return True 113 | 114 | def onEnd(self): 115 | self._pose_sub.unregister() 116 | self._pose_sub = None 117 | return True 118 | 119 | def _monitor(self, msg): 120 | self._pose = [msg.x, msg.y, msg.theta] 121 | 122 | def execute(self): 123 | if self._pose is None: 124 | return self.step("No pose received") 125 | 126 | x,y,theta = self._pose 127 | 128 | turtle = self.params["Turtle"].value 129 | turtle.setData(":Position", [x, y, 0.0]) 130 | turtle.setData(":OrientationEuler", [0.0, 0.0, theta]) 131 | self.params["Turtle"].value = turtle 132 | 133 | return self.step("") 134 | 135 | 136 | 137 | ################################################################################# 138 | # PoseController 139 | ################################################################################# 140 | 141 | class PoseController(SkillDescription): 142 | def createDescription(self): 143 | #=======Params========= 144 | self.addParam("Catch", False, ParamTypes.Required) 145 | self.addParam("Turtle", Element("cora:Robot"), ParamTypes.Required) 146 | self.addParam("Target", Element("sumo:Object"), ParamTypes.Required) 147 | self.addParam("Linear", float, ParamTypes.Optional) 148 | self.addParam("Angular", float, ParamTypes.Optional) 149 | self.addParam("MinVel", float, ParamTypes.Optional) 150 | 151 | class pose_controller(PrimitiveBase): 152 | def createDescription(self): 153 | self.setDescription(PoseController(), self.__class__.__name__) 154 | 155 | def onPreempt(self): 156 | return self.success("Preempted.") 157 | 158 | def execute(self): 159 | turtle = self.params["Turtle"].value 160 | target = self.params["Target"].value 161 | 162 | turtle_pos = np.array(turtle.getData(":Position"))[:2] 163 | target_pos = np.array(target.getData(":Position"))[:2] 164 | vec = target_pos - turtle_pos 165 | distance = np.linalg.norm(vec) 166 | 167 | if self.params["MinVel"].value is not None: 168 | distance = max(self.params["MinVel"].value, distance) 169 | 170 | if self.params["Catch"].value and distance <= 0.001: 171 | return self.success("{} caught {}".format(turtle.label, target.label)) 172 | 173 | turtle_rot = turtle.getData(":OrientationEuler")[2] 174 | 175 | a = vec / distance 176 | b = np.array([math.cos(turtle_rot), math.sin(turtle_rot)]) 177 | angle = math.acos(a.dot(b)) 178 | 179 | self.params["Linear"].value = distance / 4.0 180 | self.params["Angular"].value = math.degrees(angle) / 2.0 181 | 182 | return self.step("") 183 | 184 | -------------------------------------------------------------------------------- /src/skiros2_examples/turtlesim_example/turtlesim_skills.py: -------------------------------------------------------------------------------- 1 | from skiros2_skill.core.skill import SkillDescription, SkillBase, Serial, ParallelFf, ParallelFs, Selector, SerialStar 2 | from skiros2_common.core.params import ParamTypes 3 | from skiros2_common.core.world_element import Element 4 | 5 | import numpy as np 6 | 7 | ################################################################################# 8 | # Spawning 9 | ################################################################################# 10 | 11 | class SpawnRandom(SkillDescription): 12 | def createDescription(self): 13 | self.addParam("Name", str, ParamTypes.Required) 14 | self.addParam("RangeX", [2.0, 8.0], ParamTypes.Optional) 15 | self.addParam("RangeY", [2.0, 8.0], ParamTypes.Optional) 16 | self.addParam("RangeR", [0.0, 360.0], ParamTypes.Optional) 17 | 18 | class spawn_random(SkillBase): 19 | def createDescription(self): 20 | self.setDescription(SpawnRandom(), self.__class__.__name__) 21 | 22 | def expand(self, skill): 23 | range_x = self.params["RangeX"].values 24 | range_y = self.params["RangeY"].values 25 | range_r = self.params["RangeR"].values 26 | x = np.random.uniform(low=range_x[0], high=range_x[1]) 27 | y = np.random.uniform(low=range_y[0], high=range_y[1]) 28 | r = np.random.uniform(low=range_r[0], high=range_r[1]) 29 | 30 | skill.setProcessor(SerialStar()) 31 | skill(self.skill("Spawn", "spawn", specify={"X": x, "Y": y, "Rotation": r})) 32 | 33 | 34 | 35 | ################################################################################# 36 | # Move 37 | ################################################################################# 38 | 39 | class Move(SkillDescription): 40 | def createDescription(self): 41 | self.addParam("Turtle", Element("cora:Robot"), ParamTypes.Required) 42 | self.addParam("Distance", 0.0, ParamTypes.Required) 43 | self.addParam("Angle", 0.0, ParamTypes.Required) 44 | self.addParam("Duration", 1.0, ParamTypes.Optional) 45 | 46 | class move(SkillBase): 47 | def createDescription(self): 48 | self.setDescription(Move(), self.__class__.__name__) 49 | 50 | def expand(self, skill): 51 | t = self.params["Duration"].value 52 | v = self.params["Distance"].value / t 53 | w = self.params["Angle"].value / t 54 | uid = id(self) 55 | skill.setProcessor(ParallelFs()) 56 | skill( 57 | self.skill("Monitor", "monitor"), 58 | self.skill("Command", "command", 59 | specify={"Linear{}".format(uid): v, "Angular{}".format(uid): w}, 60 | remap={"Linear": "Linear{}".format(uid), "Angular": "Angular{}".format(uid)}), 61 | self.skill("Wait", "wait", specify={"Duration": t}) 62 | ) 63 | 64 | 65 | ################################################################################# 66 | # Patrol 67 | ################################################################################# 68 | 69 | class Patrol(SkillDescription): 70 | def createDescription(self): 71 | self.addParam("Once", True, ParamTypes.Required) 72 | self.addParam("Turtle", Element("cora:Robot"), ParamTypes.Required) 73 | 74 | class patrol(SkillBase): 75 | def createDescription(self): 76 | self.setDescription(Patrol(), self.__class__.__name__) 77 | 78 | def expand(self, skill): 79 | 80 | path = [ 81 | self.skill("Move", "move", specify={"Distance": 3.0, "Angle": 0.0, "Duration": 2.0}), 82 | self.skill("Move", "move", specify={"Distance": 0.5, "Angle": 90.0}), 83 | self.skill("Move", "move", specify={"Distance": 3.0, "Angle": 0.0, "Duration": 2.0}), 84 | self.skill("Move", "move", specify={"Distance": 0.5, "Angle": 90.0}), 85 | self.skill("Move", "move", specify={"Distance": 3.0, "Angle": 0.0, "Duration": 2.0}), 86 | self.skill("Move", "move", specify={"Distance": 0.5, "Angle": 90.0}), 87 | self.skill("Move", "move", specify={"Distance": 3.0, "Angle": 0.0, "Duration": 2.0}), 88 | self.skill("Move", "move", specify={"Distance": 0.5, "Angle": 90.0}), 89 | ] 90 | 91 | if self.params["Once"].value: 92 | skill.setProcessor(SerialStar()) 93 | skill(*path) 94 | else: 95 | skill.setProcessor(ParallelFf()) 96 | skill( 97 | self.skill(SerialStar())(*path), 98 | self.skill("Wait", "wait", specify={"Duration": 10000.0}) 99 | ) 100 | 101 | 102 | 103 | ################################################################################# 104 | # Follow 105 | ################################################################################# 106 | 107 | class Follow(SkillDescription): 108 | def createDescription(self): 109 | self.addParam("Turtle", Element("cora:Robot"), ParamTypes.Required) 110 | self.addParam("Target", Element("sumo:Object"), ParamTypes.Required) 111 | 112 | class follow(SkillBase): 113 | def createDescription(self): 114 | self.setDescription(Follow(), self.__class__.__name__) 115 | 116 | def expand(self, skill): 117 | uid = id(self) 118 | skill.setProcessor(ParallelFs()) 119 | skill( 120 | self.skill("Monitor", "monitor"), 121 | self.skill("PoseController", "pose_controller", specify={"MinVel": 2.0}, 122 | remap={"Linear": "Linear{}".format(uid), "Angular": "Angular{}".format(uid)}), 123 | self.skill("Command", "command", 124 | remap={"Linear": "Linear{}".format(uid), "Angular": "Angular{}".format(uid)}), 125 | self.skill("Wait", "wait", specify={"Duration": 10000.0}) 126 | ) 127 | 128 | 129 | 130 | ################################################################################# 131 | # Orbit 132 | ################################################################################# 133 | 134 | class Orbit(SkillDescription): 135 | def createDescription(self): 136 | self.addParam("Turtle1", Element("cora:Robot"), ParamTypes.Required) 137 | self.addParam("Turtle2", Element("cora:Robot"), ParamTypes.Required) 138 | 139 | class orbit(SkillBase): 140 | def createDescription(self): 141 | self.setDescription(Orbit(), self.__class__.__name__) 142 | 143 | def expand(self, skill): 144 | skill.setProcessor(ParallelFs()) 145 | skill( 146 | self.skill("Follow", "follow", remap={"Turtle": "Turtle1", "Target": "Turtle2"}), 147 | self.skill("Follow", "follow", remap={"Turtle": "Turtle2", "Target": "Turtle1"}), 148 | self.skill("Wait", "wait", specify={"Duration": 10000.0}) 149 | ) 150 | --------------------------------------------------------------------------------