├── README.md ├── advanced ├── CMakeLists.txt ├── TODO.md ├── include │ └── mbf_advanced │ │ ├── helpers.h │ │ └── mbf_circle_client.h ├── launch │ ├── amcl_demo_base.launch │ ├── amcl_demo_mbf.launch │ ├── amcl_demo_mbf_recovery.launch │ ├── mbf.launch │ ├── mbf_recovery.launch │ ├── rviz.launch │ └── spawn_box.launch ├── models │ └── box.urdf.xacro ├── package.xml ├── param │ ├── mbf_behavior_tree.xml │ ├── move_base_flex.yaml │ ├── move_base_flex_recovery.yaml │ ├── poses.txt │ ├── test.xml │ └── turtlebot3_navigation.rviz ├── scripts │ ├── circle_smach.py │ ├── circle_smach_continuous.py │ ├── mbf_client.py │ └── pytrees.py └── src │ ├── mbf_behavior_tree.cpp │ ├── mbf_client_node.cpp │ └── test.cpp ├── beginner ├── CMakeLists.txt ├── TODO.md ├── launch │ ├── amcl_demo_base.launch │ ├── amcl_demo_mbf.launch │ ├── amcl_demo_relay_server.launch │ ├── amcl_demo_relay_subscriber.launch │ ├── mb_relay_server.launch │ ├── mb_relay_subscriber.launch │ ├── mbf.launch │ └── rviz.launch ├── package.xml ├── param │ ├── move_base_flex.yaml │ └── turtlebot3_navigation.rviz └── scripts │ ├── mb_relay.py │ ├── mb_relay_client.py │ ├── mb_relay_server.py │ ├── mb_relay_subscriber.py │ ├── mbf_goal_client.py │ └── mbf_path_client.py └── demo.gif /README.md: -------------------------------------------------------------------------------- 1 | # Move base flex demo 2 | 3 | The repo contains the Move Base Flex Tutorials and Demos on [uos.github.io/mbf_docs](https://uos.github.io/mbf_docs) 4 | 5 | ![](./turtlebot/demo.gif) 6 | -------------------------------------------------------------------------------- /advanced/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(mbf_advanced) 3 | 4 | set(CMAKE_CXX_STANDARD 14) 5 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 6 | 7 | find_package(BehaviorTreeV3 REQUIRED) 8 | 9 | find_package(catkin REQUIRED COMPONENTS 10 | roscpp 11 | geometry_msgs 12 | actionlib 13 | ) 14 | 15 | catkin_package( 16 | DEPENDS move_base_flex 17 | ) 18 | 19 | catkin_install_python( 20 | PROGRAMS scripts/circle_smach.py scripts/pytrees.py scripts/mbf_client.py 21 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 22 | ) 23 | 24 | add_executable(mbf_client_node src/mbf_client_node.cpp) 25 | target_include_directories(mbf_client_node PRIVATE include ${catkin_INCLUDE_DIRS}) 26 | target_link_libraries(mbf_client_node ${catkin_LIBRARIES}) 27 | target_compile_definitions(mbf_client_node 28 | PRIVATE 29 | POSE_PATH="${PROJECT_SOURCE_DIR}/param/poses.txt" 30 | ) 31 | 32 | add_executable(mbf_behavior_tree src/mbf_behavior_tree.cpp) 33 | target_include_directories(mbf_behavior_tree PRIVATE include ${catkin_INCLUDE_DIRS}) 34 | target_link_libraries(mbf_behavior_tree BT::behaviortree_cpp_v3 ${catkin_LIBRARIES}) 35 | target_compile_definitions(mbf_behavior_tree 36 | PRIVATE 37 | BT_XML_PATH="${PROJECT_SOURCE_DIR}/param/mbf_behavior_tree.xml" 38 | POSE_PATH="${PROJECT_SOURCE_DIR}/param/poses.txt" 39 | ) -------------------------------------------------------------------------------- /advanced/TODO.md: -------------------------------------------------------------------------------- 1 | # TODO 2 | 3 | ## Misc 4 | - [ ] Credit rayman 5 | - [x] cancel_goals on shutdown hook 6 | - [ ] Circular Path with SimpleActionClient ((w/o) Relay) 7 | 8 | ## FAQ 9 | 10 | #### Planner Errors 11 | ``` 12 | [FATAL] [/move_base_flex]: Failed to load the base_local_planner/TrajectoryPlanner controller, are you sure it's properly registered and that the containing library is built? According to the loaded plugin descriptions the class base_local_planner/TrajectoryPlanner with base class type mbf_costmap_core::CostmapController does not exist. Declared types are According to the loaded plugin descriptions the class base_local_planner/TrajectoryPlanner with base class type nav_core::BaseLocalPlanner does not exist. Declared types are base_local_planner/TrajectoryPlannerROS dwa_local_planner/DWAPlannerROS 13 | [ERROR] [/move_base_flex]: Could not load the plugin with the name "base_local_planner/TrajectoryPlanner" and the type "base_local_planner/TrajectoryPlanner"! 14 | ``` 15 | 16 | 17 | -------------------------------------------------------------------------------- /advanced/include/mbf_advanced/helpers.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | std::ostream& operator<<(std::ostream& os, geometry_msgs::Pose pose) 12 | { 13 | return os << pose.position.x << " / " << pose.position.y << " / " << pose.position.z; 14 | } 15 | 16 | namespace mbf_advanced 17 | { 18 | 19 | std::vector loadPoseGoals(const std::string& filepath) 20 | { 21 | std::ifstream infile(filepath); 22 | std::vector pose_goals; 23 | pose_goals.reserve(8); 24 | 25 | geometry_msgs::Pose pose; 26 | 27 | while (infile >> pose.position.x 28 | >> pose.position.y 29 | >> pose.position.z 30 | >> pose.orientation.x 31 | >> pose.orientation.y 32 | >> pose.orientation.z 33 | >> pose.orientation.w) 34 | { 35 | 36 | mbf_msgs::MoveBaseGoal goal; 37 | goal.target_pose.header.frame_id = "map"; 38 | goal.target_pose.header.stamp = ros::Time::now(); 39 | goal.target_pose.pose.position = pose.position; 40 | goal.target_pose.pose.orientation = pose.orientation; 41 | pose_goals.push_back(goal); 42 | } 43 | 44 | return pose_goals; 45 | } 46 | 47 | mbf_msgs::MoveBaseResult::ConstPtr 48 | move(actionlib::SimpleActionClient &ac, const mbf_msgs::MoveBaseGoal &goal) 49 | { 50 | ac.sendGoal(goal); 51 | ac.waitForResult(); 52 | return ac.getResult(); 53 | } 54 | 55 | } // end namespace mbf_advanced -------------------------------------------------------------------------------- /advanced/include/mbf_advanced/mbf_circle_client.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace mbf_advanced 11 | { 12 | 13 | enum MBFCircleClientState 14 | { 15 | AT_END = 0, 16 | MOVING, 17 | FAILED 18 | }; 19 | 20 | struct MBFCircleClient 21 | { 22 | explicit MBFCircleClient(std::vector pose_goals) 23 | : pose_goals_(std::move(pose_goals)) 24 | , it_(pose_goals_.begin()) 25 | , prev_move_(pose_goals_.begin()) 26 | , home_(pose_goals_.back()) 27 | , ac_("move_base_flex/move_base", true) 28 | , terminate_(false) 29 | { 30 | ac_.waitForServer(); 31 | ROS_INFO("Connected to MBF action server"); 32 | } 33 | 34 | bool performCircle() 35 | { 36 | for (const auto& goal: pose_goals_) 37 | { 38 | if (!log_move(goal)) 39 | { 40 | return false; 41 | } 42 | } 43 | 44 | return true; 45 | } 46 | 47 | bool log_move(const mbf_msgs::MoveBaseGoal& goal) 48 | { 49 | ROS_INFO_STREAM("Attempting to reach " << goal.target_pose.pose.position.x << " / " << goal.target_pose.pose.position.y); 50 | auto result = mbf_advanced::move(ac_, goal); 51 | if (result->outcome != mbf_msgs::MoveBaseResult::SUCCESS) 52 | { 53 | ROS_ERROR_STREAM("Couldn't reach " << goal.target_pose.pose.position.x << " / " << goal.target_pose.pose.position.y); 54 | return false; 55 | } 56 | 57 | return true; 58 | } 59 | 60 | MBFCircleClientState next_move() 61 | { 62 | if (at_end()) 63 | { 64 | ROS_INFO_STREAM("Reached end of circle"); 65 | terminate_ = true; 66 | return MBFCircleClientState::AT_END; 67 | } 68 | 69 | prev_move_ = it_-1; 70 | auto result = log_move(*it_); 71 | ++it_; 72 | return result ? MBFCircleClientState::MOVING : MBFCircleClientState::FAILED; 73 | } 74 | 75 | bool at_end() 76 | { 77 | return it_ == pose_goals_.end(); 78 | } 79 | 80 | MBFCircleClientState prev_move() 81 | { 82 | if (terminate_) 83 | { 84 | return MBFCircleClientState::AT_END; 85 | } 86 | 87 | if (prev_move_ != pose_goals_.begin()) 88 | { 89 | auto result = log_move(*prev_move_); 90 | it_ = prev_move_+1; 91 | --prev_move_; 92 | return result ? MBFCircleClientState::MOVING : MBFCircleClientState::FAILED; 93 | } 94 | else 95 | { 96 | return driveHome() ? MBFCircleClientState::MOVING : MBFCircleClientState::FAILED; 97 | } 98 | } 99 | 100 | bool driveHome() 101 | { 102 | ROS_WARN_STREAM("Driving home!"); 103 | return log_move(home_); 104 | } 105 | 106 | std::vector pose_goals_; 107 | std::vector::const_iterator it_; 108 | std::vector::const_iterator prev_move_; 109 | const mbf_msgs::MoveBaseGoal& home_; 110 | actionlib::SimpleActionClient ac_; 111 | bool terminate_; 112 | }; 113 | 114 | } // end -------------------------------------------------------------------------------- /advanced/launch/amcl_demo_base.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 | -------------------------------------------------------------------------------- /advanced/launch/amcl_demo_mbf.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /advanced/launch/amcl_demo_mbf_recovery.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /advanced/launch/mbf.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /advanced/launch/mbf_recovery.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /advanced/launch/rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | -------------------------------------------------------------------------------- /advanced/launch/spawn_box.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /advanced/models/box.urdf.xacro: -------------------------------------------------------------------------------- 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 | 29 | 30 | 31 | 32 | 33 | 1 34 | 35 | 36 | 37 | Gazebo/Blue 38 | 39 | -------------------------------------------------------------------------------- /advanced/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mbf_advanced 4 | 0.0.0 5 | Minimal examples for move base flex and turtlebot 6 | 7 | Julian Gaal 8 | 9 | TODO 10 | 11 | catkin 12 | turtlebot3_navigation 13 | turtlebot3_gazebo 14 | dwa_local_planner 15 | eband_local_planner 16 | move_base_flex 17 | clear_costmap_recovery 18 | rotate_recovery 19 | behaviortree_cpp_v3 20 | py_trees_ros 21 | moveback_recovery 22 | 23 | -------------------------------------------------------------------------------- /advanced/param/mbf_behavior_tree.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /advanced/param/move_base_flex.yaml: -------------------------------------------------------------------------------- 1 | planners: 2 | - name: navfn/NavfnROS 3 | type: navfn/NavfnROS 4 | 5 | controllers: 6 | - name: eband_local_planner/EBandPlannerROS 7 | type: eband_local_planner/EBandPlannerROS 8 | 9 | controller_frequency: 5.0 10 | controller_patience: 10.0 11 | controller_max_retries: 10 12 | 13 | planner_frequency: 1 14 | planner_patience: 10.0 15 | planner_max_retries: 10 16 | 17 | recovery_enabled: false 18 | 19 | oscillation_timeout: 10.0 20 | oscillation_distance: 0.2 21 | -------------------------------------------------------------------------------- /advanced/param/move_base_flex_recovery.yaml: -------------------------------------------------------------------------------- 1 | planners: 2 | - name: navfn/NavfnROS 3 | type: navfn/NavfnROS 4 | 5 | controllers: 6 | - name: eband_local_planner/EBandPlannerROS 7 | type: eband_local_planner/EBandPlannerROS 8 | 9 | recovery_behaviors: 10 | - name: rotate_recovery 11 | type: rotate_recovery/RotateRecovery 12 | - name: clear_costmap_recovery 13 | type: clear_costmap_recovery/ClearCostmapRecovery 14 | 15 | controller_frequency: 5.0 16 | controller_patience: 10.0 17 | controller_max_retries: 10 18 | 19 | planner_frequency: 1 20 | planner_patience: 10.0 21 | planner_max_retries: 10 22 | 23 | recovery_enabled: true 24 | recovery_patience: 15.0 25 | 26 | oscillation_timeout: 10.0 27 | oscillation_distance: 0.2 28 | -------------------------------------------------------------------------------- /advanced/param/poses.txt: -------------------------------------------------------------------------------- 1 | -1.75 0.74 0 0 0 0.539 0.843 2 | -0.36 1.92 0 0 0 -0.020 0.999 3 | 0.957 1.60 0 0 0 -0.163 0.987 4 | 1.8741 0.3830 0 0 0 -0.70 0.711 5 | 1.752 -0.928 0 0 0 -0.856 0.517 6 | 0.418 -2.116 0 0 0 0.998 0.0619 7 | -0.775 -1.80 0 0 0 0.954 0.300 8 | -1.990 -0.508 0 0 0 -0.100 0.999 -------------------------------------------------------------------------------- /advanced/param/test.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /advanced/param/turtlebot3_navigation.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Planner Path1 8 | - /Path1/Status1 9 | Splitter Ratio: 0.5 10 | Tree Height: 1172 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | Name: Tool Properties 18 | Splitter Ratio: 0.5886790156364441 19 | - Class: rviz/Views 20 | Expanded: 21 | - /Current View1 22 | Name: Views 23 | Splitter Ratio: 0.5 24 | - Class: rviz/Time 25 | Experimental: false 26 | Name: Time 27 | SyncMode: 0 28 | SyncSource: LaserScan 29 | Preferences: 30 | PromptSaveOnExit: true 31 | Toolbars: 32 | toolButtonStyle: 2 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.029999999329447746 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: 20 52 | Reference Frame: 53 | Value: true 54 | - Alpha: 1 55 | Class: rviz/RobotModel 56 | Collision Enabled: false 57 | Enabled: true 58 | Links: 59 | All Links Enabled: true 60 | Expand Joint Details: false 61 | Expand Link Details: false 62 | Expand Tree: false 63 | Link Tree Style: Links in Alphabetic Order 64 | base_footprint: 65 | Alpha: 1 66 | Show Axes: false 67 | Show Trail: false 68 | base_link: 69 | Alpha: 1 70 | Show Axes: false 71 | Show Trail: false 72 | Value: true 73 | base_scan: 74 | Alpha: 1 75 | Show Axes: false 76 | Show Trail: false 77 | Value: true 78 | caster_back_link: 79 | Alpha: 1 80 | Show Axes: false 81 | Show Trail: false 82 | Value: true 83 | imu_link: 84 | Alpha: 1 85 | Show Axes: false 86 | Show Trail: false 87 | wheel_left_link: 88 | Alpha: 1 89 | Show Axes: false 90 | Show Trail: false 91 | Value: true 92 | wheel_right_link: 93 | Alpha: 1 94 | Show Axes: false 95 | Show Trail: false 96 | Value: true 97 | Name: RobotModel 98 | Robot Description: robot_description 99 | TF Prefix: "" 100 | Update Interval: 0 101 | Value: true 102 | Visual Enabled: true 103 | - Alpha: 1 104 | Autocompute Intensity Bounds: true 105 | Autocompute Value Bounds: 106 | Max Value: 10 107 | Min Value: -10 108 | Value: true 109 | Axis: Z 110 | Channel Name: intensity 111 | Class: rviz/LaserScan 112 | Color: 0; 255; 0 113 | Color Transformer: FlatColor 114 | Decay Time: 0 115 | Enabled: true 116 | Invert Rainbow: false 117 | Max Color: 255; 255; 255 118 | Max Intensity: 13069 119 | Min Color: 0; 0; 0 120 | Min Intensity: 28 121 | Name: LaserScan 122 | Position Transformer: XYZ 123 | Queue Size: 10 124 | Selectable: true 125 | Size (Pixels): 3 126 | Size (m): 0.030000001192092896 127 | Style: Flat Squares 128 | Topic: /scan 129 | Unreliable: false 130 | Use Fixed Frame: true 131 | Use rainbow: true 132 | Value: true 133 | - Alpha: 0.699999988079071 134 | Class: rviz/Map 135 | Color Scheme: map 136 | Draw Behind: false 137 | Enabled: true 138 | Name: Map 139 | Topic: /map 140 | Unreliable: false 141 | Use Timestamp: false 142 | Value: true 143 | - Class: rviz/Group 144 | Displays: 145 | - Alpha: 0.699999988079071 146 | Class: rviz/Map 147 | Color Scheme: costmap 148 | Draw Behind: true 149 | Enabled: true 150 | Name: Costmap 151 | Topic: /move_base_flex/global_costmap/costmap 152 | Unreliable: false 153 | Use Timestamp: false 154 | Value: true 155 | - Alpha: 1 156 | Buffer Length: 1 157 | Class: rviz/Path 158 | Color: 255; 0; 0 159 | Enabled: true 160 | Head Diameter: 0.30000001192092896 161 | Head Length: 0.20000000298023224 162 | Length: 0.30000001192092896 163 | Line Style: Lines 164 | Line Width: 0.029999999329447746 165 | Name: Planner 166 | Offset: 167 | X: 0 168 | Y: 0 169 | Z: 0 170 | Pose Color: 255; 85; 255 171 | Pose Style: None 172 | Queue Size: 10 173 | Radius: 0.029999999329447746 174 | Shaft Diameter: 0.10000000149011612 175 | Shaft Length: 0.10000000149011612 176 | Topic: /move_base_flex/NavfnROS/plan 177 | Unreliable: false 178 | Value: true 179 | Enabled: true 180 | Name: Global Map 181 | - Class: rviz/Group 182 | Displays: 183 | - Alpha: 1 184 | Class: rviz/Polygon 185 | Color: 0; 0; 0 186 | Enabled: true 187 | Name: Polygon 188 | Queue Size: 10 189 | Topic: /move_base_flex/local_costmap/footprint 190 | Unreliable: false 191 | Value: true 192 | - Alpha: 0.699999988079071 193 | Class: rviz/Map 194 | Color Scheme: costmap 195 | Draw Behind: false 196 | Enabled: true 197 | Name: Costmap 198 | Topic: /move_base_flex/local_costmap/costmap 199 | Unreliable: false 200 | Use Timestamp: false 201 | Value: true 202 | - Alpha: 1 203 | Buffer Length: 1 204 | Class: rviz/Path 205 | Color: 255; 255; 0 206 | Enabled: true 207 | Head Diameter: 0.30000001192092896 208 | Head Length: 0.20000000298023224 209 | Length: 0.30000001192092896 210 | Line Style: Lines 211 | Line Width: 0.029999999329447746 212 | Name: Planner 213 | Offset: 214 | X: 0 215 | Y: 0 216 | Z: 0 217 | Pose Color: 255; 85; 255 218 | Pose Style: None 219 | Queue Size: 10 220 | Radius: 0.029999999329447746 221 | Shaft Diameter: 0.10000000149011612 222 | Shaft Length: 0.10000000149011612 223 | Topic: /move_base_flex/DWAPlannerROS/local_plan 224 | Unreliable: false 225 | Value: true 226 | Enabled: true 227 | Name: Local Map 228 | - Alpha: 1 229 | Arrow Length: 0.05000000074505806 230 | Axes Length: 0.30000001192092896 231 | Axes Radius: 0.009999999776482582 232 | Class: rviz/PoseArray 233 | Color: 0; 192; 0 234 | Enabled: true 235 | Head Length: 0.07000000029802322 236 | Head Radius: 0.029999999329447746 237 | Name: Amcl Particles 238 | Queue Size: 10 239 | Shaft Length: 0.23000000417232513 240 | Shaft Radius: 0.009999999776482582 241 | Shape: Arrow (Flat) 242 | Topic: /particlecloud 243 | Unreliable: false 244 | Value: true 245 | - Alpha: 1 246 | Buffer Length: 1 247 | Class: rviz/Path 248 | Color: 255; 0; 0 249 | Enabled: true 250 | Head Diameter: 0.30000001192092896 251 | Head Length: 0.20000000298023224 252 | Length: 0.30000001192092896 253 | Line Style: Billboards 254 | Line Width: 0.029999999329447746 255 | Name: Planner Path 256 | Offset: 257 | X: 0 258 | Y: 0 259 | Z: 0 260 | Pose Color: 255; 85; 255 261 | Pose Style: None 262 | Queue Size: 10 263 | Radius: 0.029999999329447746 264 | Shaft Diameter: 0.10000000149011612 265 | Shaft Length: 0.10000000149011612 266 | Topic: /move_base_flex/navfn/NavfnROS/plan 267 | Unreliable: false 268 | Value: true 269 | - Alpha: 1 270 | Axes Length: 1 271 | Axes Radius: 0.10000000149011612 272 | Class: rviz/Pose 273 | Color: 255; 25; 0 274 | Enabled: true 275 | Head Length: 0.10000000149011612 276 | Head Radius: 0.20000000298023224 277 | Name: Target Pose 278 | Queue Size: 10 279 | Shaft Length: 0.30000001192092896 280 | Shaft Radius: 0.10000000149011612 281 | Shape: Arrow 282 | Topic: /move_base_flex/current_goal 283 | Unreliable: false 284 | Value: true 285 | - Alpha: 1 286 | Buffer Length: 1 287 | Class: rviz/Path 288 | Color: 25; 255; 0 289 | Enabled: true 290 | Head Diameter: 0.30000001192092896 291 | Head Length: 0.20000000298023224 292 | Length: 0.30000001192092896 293 | Line Style: Lines 294 | Line Width: 0.029999999329447746 295 | Name: Path 296 | Offset: 297 | X: 0 298 | Y: 0 299 | Z: 0 300 | Pose Color: 255; 85; 255 301 | Pose Style: None 302 | Queue Size: 10 303 | Radius: 0.029999999329447746 304 | Shaft Diameter: 0.10000000149011612 305 | Shaft Length: 0.10000000149011612 306 | Topic: /move_base_flex/eband_local_planner/EBandPlannerROS/local_plan 307 | Unreliable: false 308 | Value: true 309 | - Alpha: 1 310 | Buffer Length: 1 311 | Class: rviz/Path 312 | Color: 25; 255; 0 313 | Enabled: true 314 | Head Diameter: 0.30000001192092896 315 | Head Length: 0.20000000298023224 316 | Length: 0.30000001192092896 317 | Line Style: Lines 318 | Line Width: 0.029999999329447746 319 | Name: Path 320 | Offset: 321 | X: 0 322 | Y: 0 323 | Z: 0 324 | Pose Color: 255; 85; 255 325 | Pose Style: None 326 | Queue Size: 10 327 | Radius: 0.029999999329447746 328 | Shaft Diameter: 0.10000000149011612 329 | Shaft Length: 0.10000000149011612 330 | Topic: /move_base_flex/eband_local_planner/EBandPlannerROS/global_plan 331 | Unreliable: false 332 | Value: true 333 | Enabled: true 334 | Global Options: 335 | Background Color: 48; 48; 48 336 | Default Light: true 337 | Fixed Frame: map 338 | Frame Rate: 30 339 | Name: root 340 | Tools: 341 | - Class: rviz/MoveCamera 342 | - Class: rviz/Interact 343 | Hide Inactive Objects: true 344 | - Class: rviz/Select 345 | - Class: rviz/SetInitialPose 346 | Theta std deviation: 0.2617993950843811 347 | Topic: /initialpose 348 | X std deviation: 0.5 349 | Y std deviation: 0.5 350 | - Class: rviz/SetGoal 351 | Topic: /move_base_simple/goal 352 | - Class: rviz/Measure 353 | Value: true 354 | Views: 355 | Current: 356 | Angle: -4.6707916259765625 357 | Class: rviz/TopDownOrtho 358 | Enable Stereo Rendering: 359 | Stereo Eye Separation: 0.05999999865889549 360 | Stereo Focal Distance: 1 361 | Swap Stereo Eyes: false 362 | Value: false 363 | Invert Z Axis: false 364 | Name: Current View 365 | Near Clip Distance: 0.009999999776482582 366 | Scale: -126.7267837524414 367 | Target Frame: 368 | X: -0.30647900700569153 369 | Y: -0.2619345784187317 370 | Saved: ~ 371 | Window Geometry: 372 | Displays: 373 | collapsed: false 374 | Height: 1404 375 | Hide Left Dock: false 376 | Hide Right Dock: true 377 | QMainWindow State: 000000ff00000000fd0000000400000000000001a100000520fc0200000007fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003e00000520000000ca00fffffffb0000000a0049006d0061006700650000000317000000cc0000000000000000fb0000000a0049006d0061006700650000000330000000ce0000000000000000000000010000010f000003a0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000043000003a0000000a600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002b900fffffffb0000000800540069006d006501000000000000045000000000000000000000034f0000052000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 378 | Selection: 379 | collapsed: false 380 | Time: 381 | collapsed: false 382 | Tool Properties: 383 | collapsed: false 384 | Views: 385 | collapsed: true 386 | Width: 1270 387 | X: 1282 388 | Y: 4 389 | -------------------------------------------------------------------------------- /advanced/scripts/circle_smach.py: -------------------------------------------------------------------------------- 1 | import rospy 2 | import smach 3 | import smach_ros 4 | import threading 5 | 6 | from geometry_msgs.msg import PoseStamped 7 | from nav_msgs.msg import Path 8 | 9 | from mbf_msgs.msg import ExePathAction 10 | from mbf_msgs.msg import GetPathAction 11 | from mbf_msgs.msg import RecoveryAction 12 | 13 | 14 | def create_pose(x, y, z, xx, yy, zz, ww): 15 | pose = PoseStamped() 16 | pose.header.frame_id = "map" 17 | pose.header.stamp = rospy.Time.now() 18 | pose.pose.position.x = x 19 | pose.pose.position.y = y 20 | pose.pose.position.z = z 21 | pose.pose.orientation.x = xx 22 | pose.pose.orientation.y = yy 23 | pose.pose.orientation.z = zz 24 | pose.pose.orientation.w = ww 25 | return pose 26 | 27 | 28 | def iterate_target_poses(): 29 | target_poses = [ 30 | create_pose(-1.75, 0.74, 0, 0, 0, 0.539, 0.843), 31 | create_pose(-0.36, 1.92, 0, 0, 0, -0.020, 0.999), 32 | create_pose(0.957, 1.60, 0, 0, 0, -0.163, 0.987), 33 | create_pose(1.8741, 0.3830, 0, 0, 0, -0.70, 0.711), 34 | create_pose(1.752, -0.928, 0, 0, 0, -0.856, 0.517), 35 | create_pose(0.418, -2.116, 0, 0, 0, 0.998, 0.0619), 36 | create_pose(-0.775, -1.80, 0, 0, 0, 0.954, 0.300), 37 | create_pose(-1.990, -0.508, 0, 0, 0, -0.112, 0.999) 38 | ] 39 | 40 | for target_pose in target_poses: 41 | yield target_pose 42 | 43 | def create_path_goal(path, tolerance_from_action, dist_tolerance, angle_tolerance): 44 | goal = mbf_msgs.ExePathGoal() 45 | goal.path = path 46 | goal.tolerance_from_action = tolerance_from_action 47 | goal.dist_tolerance = dist_tolerance 48 | goal.angle_tolerance = angle_tolerance 49 | return goal 50 | 51 | def main(): 52 | rospy.init_node('mbf_state_machine') 53 | 54 | target_poses = iterate_target_poses() 55 | 56 | # Create SMACH state machine 57 | sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted']) 58 | 59 | # Define userdata 60 | sm.userdata.target_pose = None 61 | sm.userdata.path = None 62 | 63 | with sm: 64 | # path callback 65 | def get_path_callback(userdata, goal): 66 | try: 67 | goal.target_pose = next(target_poses) 68 | except StopIteration: 69 | rospy.logwarn("Reached last target pose") 70 | rospy.signal_shutdown("Last goal reached. Shutting down") 71 | 72 | # Get path 73 | smach.StateMachine.add( 74 | 'GET_PATH', 75 | smach_ros.SimpleActionState( 76 | '/move_base_flex/get_path', 77 | GetPathAction, 78 | goal_cb=get_path_callback, 79 | goal_slots=['target_pose'], 80 | result_slots=['path'] 81 | ), 82 | transitions={ 83 | 'succeeded': 'EXE_PATH', 84 | 'aborted': 'aborted', 85 | 'preempted': 'preempted' 86 | } 87 | ) 88 | 89 | def exe_path_callback(userdata, goal): 90 | target_pose = goal.path.poses[-1].pose 91 | rospy.loginfo("Attempting to reach (%1.3f, %1.3f)", target_pose.position.x, target_pose.position.y) 92 | 93 | # Execute path 94 | smach.StateMachine.add( 95 | 'EXE_PATH', 96 | smach_ros.SimpleActionState( 97 | '/move_base_flex/exe_path', 98 | ExePathAction, 99 | goal_cb=exe_path_callback, 100 | goal_slots=['path'] 101 | ), 102 | transitions={ 103 | 'succeeded': 'GET_PATH', 104 | 'aborted': 'aborted', 105 | 'preempted': 'preempted' 106 | } 107 | ) 108 | 109 | # Execute SMACH plan 110 | # Create a thread to execute the smach container 111 | smach_thread = threading.Thread(target=sm.execute) 112 | smach_thread.start() 113 | 114 | # Wait for ctrl-c 115 | rospy.spin() 116 | 117 | # Request the container to preempt 118 | sm.request_preempt() 119 | 120 | # Block until everything is preempted 121 | smach_thread.join() 122 | 123 | if __name__=="__main__": 124 | main() -------------------------------------------------------------------------------- /advanced/scripts/circle_smach_continuous.py: -------------------------------------------------------------------------------- 1 | import rospy 2 | import smach 3 | import smach_ros 4 | import threading 5 | 6 | from geometry_msgs.msg import PoseStamped 7 | from nav_msgs.msg import Path 8 | 9 | from mbf_msgs.msg import ExePathAction 10 | from mbf_msgs.msg import GetPathAction 11 | from mbf_msgs.msg import RecoveryAction 12 | 13 | 14 | def create_pose(x, y, z, xx, yy, zz, ww): 15 | pose = PoseStamped() 16 | pose.header.frame_id = "map" 17 | pose.header.stamp = rospy.Time.now() 18 | pose.pose.position.x = x 19 | pose.pose.position.y = y 20 | pose.pose.position.z = z 21 | pose.pose.orientation.x = xx 22 | pose.pose.orientation.y = yy 23 | pose.pose.orientation.z = zz 24 | pose.pose.orientation.w = ww 25 | return pose 26 | 27 | 28 | def iterate_target_poses(): 29 | target_poses = [ 30 | create_pose(-1.75, 0.74, 0, 0, 0, 0.539, 0.843), 31 | create_pose(-0.36, 1.92, 0, 0, 0, -0.020, 0.999), 32 | create_pose(0.957, 1.60, 0, 0, 0, -0.163, 0.987), 33 | create_pose(1.8741, 0.3830, 0, 0, 0, -0.70, 0.711), 34 | create_pose(1.752, -0.928, 0, 0, 0, -0.856, 0.517), 35 | create_pose(0.418, -2.116, 0, 0, 0, 0.998, 0.0619), 36 | create_pose(-0.775, -1.80, 0, 0, 0, 0.954, 0.300), 37 | create_pose(-1.990, -0.508, 0, 0, 0, -0.112, 0.999) 38 | ] 39 | 40 | for target_pose in target_poses: 41 | yield target_pose 42 | 43 | def create_path_goal(path, tolerance_from_action, dist_tolerance, angle_tolerance): 44 | goal = mbf_msgs.ExePathGoal() 45 | goal.path = path 46 | goal.tolerance_from_action = tolerance_from_action 47 | goal.dist_tolerance = dist_tolerance 48 | goal.angle_tolerance = angle_tolerance 49 | return goal 50 | 51 | def main(): 52 | rospy.init_node('mbf_state_machine') 53 | 54 | target_poses = iterate_target_poses() 55 | 56 | # Create SMACH state machine 57 | sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted']) 58 | 59 | # Define userdata 60 | sm.userdata.target_pose = None 61 | sm.userdata.path = None 62 | sm.userdata.error = None 63 | sm.userdata.clear_costmap_flag = False 64 | sm.userdata.error_status = None 65 | 66 | with sm: 67 | # path callback 68 | def get_path_callback(userdata, goal): 69 | try: 70 | goal.target_pose = next(target_poses) 71 | except StopIteration: 72 | rospy.logwarn("Reached last target pose") 73 | rospy.signal_shutdown("Last goal reached. Shutting down") 74 | 75 | # Get path 76 | smach.StateMachine.add( 77 | 'GET_PATH', 78 | smach_ros.SimpleActionState( 79 | '/move_base_flex/get_path', 80 | GetPathAction, 81 | goal_cb=get_path_callback, 82 | goal_slots=['target_pose'], 83 | result_slots=['path'] 84 | ), 85 | transitions={ 86 | 'succeeded': 'EXE_PATH', 87 | 'aborted': 'FAILED', 88 | 'preempted': 'preempted' 89 | } 90 | ) 91 | 92 | def path_callback(userdata, goal): 93 | target_pose = goal.path.poses[-1].pose 94 | rospy.loginfo("Attempting to reach (%1.3f, %1.3f)", target_pose.position.x, target_pose.position.y) 95 | 96 | # Execute path 97 | smach.StateMachine.add( 98 | 'EXE_PATH', 99 | smach_ros.SimpleActionState( 100 | '/move_base_flex/exe_path', 101 | ExePathAction, 102 | goal_cb=path_callback, 103 | goal_slots=['path'] 104 | ), 105 | transitions={ 106 | 'succeeded': 'GET_PATH', 107 | 'aborted': 'FAILED', 108 | 'preempted': 'preempted' 109 | } 110 | ) 111 | 112 | smach.StateMachine.add( 113 | 'FAILED', 114 | 115 | ) 116 | 117 | # Execute SMACH plan 118 | # Create a thread to execute the smach container 119 | smach_thread = threading.Thread(target=sm.execute) 120 | smach_thread.start() 121 | 122 | # Wait for ctrl-c 123 | rospy.spin() 124 | 125 | # Request the container to preempt 126 | sm.request_preempt() 127 | 128 | # Block until everything is preempted 129 | smach_thread.join() 130 | 131 | if __name__=="__main__": 132 | main() -------------------------------------------------------------------------------- /advanced/scripts/mbf_client.py: -------------------------------------------------------------------------------- 1 | import rospy 2 | import mbf_msgs.msg as mbf_msgs 3 | 4 | def create_goal(x, y, z, xx, yy, zz, ww): 5 | goal = mbf_msgs.MoveBaseGoal() 6 | goal.target_pose.header.frame_id = "map" 7 | goal.target_pose.header.stamp = rospy.Time.now() 8 | goal.target_pose.pose.position.x = x 9 | goal.target_pose.pose.position.y = y 10 | goal.target_pose.pose.position.z = z 11 | goal.target_pose.pose.orientation.x = xx 12 | goal.target_pose.pose.orientation.y = yy 13 | goal.target_pose.pose.orientation.z = zz 14 | goal.target_pose.pose.orientation.w = ww 15 | return goal 16 | 17 | 18 | def goal_generator(): 19 | target_poses = [ 20 | create_goal(-1.75, 0.74, 0, 0, 0, 0.539, 0.843), 21 | create_goal(-0.36, 1.92, 0, 0, 0, -0.020, 0.999), 22 | create_goal(0.957, 1.60, 0, 0, 0, -0.163, 0.987), 23 | create_goal(1.8741, 0.3830, 0, 0, 0, -0.70, 0.711), 24 | create_goal(1.752, -0.928, 0, 0, 0, -0.856, 0.517), 25 | create_goal(0.418, -2.116, 0, 0, 0, 0.998, 0.0619), 26 | create_goal(-0.775, -1.80, 0, 0, 0, 0.954, 0.300), 27 | create_goal(-1.990, -0.508, 0, 0, 0, -0.112, 0.999) 28 | ] 29 | 30 | for target_pose in target_poses: 31 | yield target_pose 32 | 33 | 34 | class Goals: 35 | def __init__(self): 36 | self.goals = [ 37 | create_goal(-1.75, 0.74, 0, 0, 0, 0.539, 0.843), 38 | create_goal(-0.36, 1.92, 0, 0, 0, -0.020, 0.999), 39 | create_goal(0.957, 1.60, 0, 0, 0, -0.163, 0.987), 40 | create_goal(1.8741, 0.3830, 0, 0, 0, -0.70, 0.711), 41 | create_goal(1.752, -0.928, 0, 0, 0, -0.856, 0.517), 42 | create_goal(0.418, -2.116, 0, 0, 0, 0.998, 0.0619), 43 | create_goal(-0.775, -1.80, 0, 0, 0, 0.954, 0.300), 44 | create_goal(-1.990, -0.508, 0, 0, 0, -0.112, 0.999) 45 | ] 46 | 47 | self.iterator = 0 48 | self.next_called = False 49 | 50 | def next(self): 51 | goal = self.goals[self.iterator] 52 | self.iterator += 1 53 | self.next_called = True 54 | 55 | if self.iterator > len(self.goals)-1: 56 | raise IndexError 57 | 58 | return goal 59 | 60 | def prev(self): 61 | if self.next_called: 62 | self.iterator -= 2 63 | self.next_called = False 64 | else: 65 | self.iterator -= 1 66 | 67 | if self.iterator < 0: 68 | raise IndexError 69 | 70 | return self.goals[self.iterator] 71 | 72 | class MBFClient: 73 | def __init__(self): 74 | self.goals = Goals() 75 | 76 | def next(self): 77 | return self.goals.next() 78 | 79 | def prev(self): 80 | return self.goals.prev() 81 | 82 | if __name__ == '__main__': 83 | rospy.init_node("move_base_flex_client") 84 | mbf = MBFClient() -------------------------------------------------------------------------------- /advanced/scripts/pytrees.py: -------------------------------------------------------------------------------- 1 | """ 2 | MBF BT Demo: Behavior tree implementing a really basic navigation strategy, 3 | even simpler than the move_base hardcoded FSM, as it lacks: 4 | 5 | * continuous replanning 6 | * oscillation detection 7 | 8 | We create on the first place action client behaviors for MBF's planner, controller and recovery action servers 9 | On this simple demo we need to add pretty little additional code to the base ActionClient class 10 | """ 11 | 12 | ############################################################################## 13 | # Imports 14 | ############################################################################## 15 | 16 | import functools 17 | import py_trees 18 | import py_trees_ros 19 | import py_trees.console as console 20 | import rospy 21 | import sys 22 | 23 | import geometry_msgs.msg as geometry_msgs 24 | import mbf_msgs.msg as mbf_msgs 25 | 26 | 27 | ############################################################################## 28 | # Actions 29 | ############################################################################## 30 | 31 | class GetPath(py_trees_ros.actions.ActionClient): 32 | 33 | def initialise(self): 34 | """ 35 | Get target pose from the blackboard to create an action goal 36 | """ 37 | self.action_goal = mbf_msgs.GetPathGoal(target_pose=py_trees.blackboard.Blackboard().get("target_pose")) 38 | super(GetPath, self).initialise() 39 | 40 | def update(self): 41 | """ 42 | On success, set the resulting path on the blackboard, so ExePath can use it 43 | """ 44 | status = super(GetPath, self).update() 45 | if status == py_trees.Status.SUCCESS: 46 | py_trees.blackboard.Blackboard().set("path", self.action_client.get_result().path) 47 | return status 48 | 49 | class ExePath(py_trees_ros.actions.ActionClient): 50 | 51 | def initialise(self): 52 | """ 53 | Get path from the blackboard to create an action goal 54 | """ 55 | self.action_goal = mbf_msgs.ExePathGoal(path=py_trees.blackboard.Blackboard().get("path")) 56 | super(ExePath, self).initialise() 57 | 58 | class Recovery(py_trees_ros.actions.ActionClient): 59 | def setup(self, timeout): 60 | """ 61 | Read the list of available recovery behaviors so we can try them in sequence 62 | """ 63 | self._behaviors = rospy.get_param("/move_base_flex/recovery_behaviors") 64 | return super(Recovery, self).setup(timeout) 65 | 66 | def update(self): 67 | """ 68 | Try the next recovery behavior, dropping it from the list 69 | """ 70 | try: 71 | self.action_goal = mbf_msgs.RecoveryGoal(behavior=self._behaviors.pop(0)["name"]) 72 | return super(Recovery, self).update() 73 | except IndexError: 74 | # recovery behaviors exhausted; fail to abort navigation but restore the list for the next goal 75 | # TODO: this means that we won't reset the list after a successful recovery, so the list keeps shrinking 76 | # until fully exhausted; that's clearly not the expected operation, so I need to find a better solution 77 | self._behaviors = rospy.get_param("/move_base_flex/recovery_behaviors") 78 | return py_trees.Status.FAILURE 79 | 80 | 81 | ############################################################################## 82 | # Behaviours 83 | ############################################################################## 84 | 85 | def create_root(): 86 | # Create all behaviours 87 | bt_root = py_trees.composites.Sequence("MBF BT Demo") 88 | get_goal = py_trees.composites.Selector("GetGoal") 89 | fallback = py_trees.composites.Selector("Fallback") 90 | navigate = py_trees.composites.Sequence("Navigate") 91 | new_goal = py_trees_ros.subscribers.ToBlackboard(name="NewGoal", 92 | topic_name="/move_base_simple/goal", 93 | topic_type=geometry_msgs.PoseStamped, 94 | blackboard_variables = {'target_pose': None}) 95 | have_goal = py_trees.blackboard.CheckBlackboardVariable(name="HaveGoal", variable_name="target_pose") 96 | clr_goal1 = py_trees.blackboard.ClearBlackboardVariable(name="ClearGoal", variable_name="target_pose") 97 | clr_goal2 = py_trees.blackboard.ClearBlackboardVariable(name="ClearGoal", variable_name="target_pose") 98 | get_path = GetPath(name="GetPath", 99 | action_namespace="/move_base_flex/get_path", 100 | action_spec=mbf_msgs.GetPathAction) 101 | exe_path = ExePath(name="ExePath", 102 | action_namespace="/move_base_flex/exe_path", 103 | action_spec=mbf_msgs.ExePathAction) 104 | recovery = Recovery(name="Recovery", 105 | action_namespace="/move_base_flex/recovery", 106 | action_spec=mbf_msgs.RecoveryAction) 107 | 108 | # Compose tree 109 | bt_root.add_children([get_goal, fallback]) 110 | get_goal.add_children([have_goal, new_goal]) 111 | navigate.add_children([get_path, exe_path, clr_goal1]) 112 | fallback.add_children([navigate, recovery, clr_goal2]) 113 | return bt_root 114 | 115 | 116 | def shutdown(behaviour_tree): 117 | behaviour_tree.interrupt() 118 | 119 | if __name__ == '__main__': 120 | rospy.init_node("mbf_bt_demo") 121 | root = create_root() 122 | behaviour_tree = py_trees_ros.trees.BehaviourTree(root) 123 | rospy.on_shutdown(functools.partial(shutdown, behaviour_tree)) 124 | if not behaviour_tree.setup(timeout=15): 125 | console.logerror("failed to setup the tree, aborting.") 126 | sys.exit(1) 127 | 128 | behaviour_tree.tick_tock(500) -------------------------------------------------------------------------------- /advanced/src/mbf_behavior_tree.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | using State = mbf_advanced::MBFCircleClientState; 7 | 8 | BT::NodeStatus DriveHome(std::shared_ptr& mbfclient) 9 | { 10 | ROS_INFO_STREAM("BT: driving home"); 11 | return mbfclient->driveHome() ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE; 12 | } 13 | 14 | class AttemptNext : public BT::SyncActionNode 15 | { 16 | public: 17 | explicit AttemptNext(const std::string& name) 18 | : BT::SyncActionNode(name, {}) 19 | , mbfclient_{} 20 | { } 21 | 22 | void attachMBFClient(std::shared_ptr mbfclient) 23 | { 24 | mbfclient_ = mbfclient; 25 | } 26 | 27 | BT::NodeStatus tick() override 28 | { 29 | if (mbfclient_) 30 | { 31 | ROS_INFO_STREAM("BT: " << this->name()); 32 | 33 | while (mbfclient_->next_move() == State::MOVING) {} 34 | 35 | return BT::NodeStatus::FAILURE; 36 | } 37 | 38 | return BT::NodeStatus::FAILURE; 39 | } 40 | 41 | private: 42 | std::shared_ptr mbfclient_; 43 | }; 44 | 45 | class AttemptSkip : public BT::SyncActionNode 46 | { 47 | public: 48 | explicit AttemptSkip(const std::string& name) 49 | : BT::SyncActionNode(name, {}) 50 | , mbfclient_{} 51 | { } 52 | 53 | void attachMBFClient(std::shared_ptr mbfclient) 54 | { 55 | mbfclient_ = mbfclient; 56 | } 57 | 58 | BT::NodeStatus tick() override 59 | { 60 | if (mbfclient_) 61 | { 62 | ROS_INFO_STREAM("BT: " << this->name()); 63 | return (mbfclient_->next_move() == State::MOVING) ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE; 64 | } 65 | 66 | return BT::NodeStatus::FAILURE; 67 | } 68 | 69 | private: 70 | std::shared_ptr mbfclient_; 71 | }; 72 | 73 | class AttemptPrevious : public BT::SyncActionNode 74 | { 75 | public: 76 | AttemptPrevious(const std::string& name) 77 | : SyncActionNode(name, {}) 78 | , mbfclient_{} 79 | { } 80 | 81 | void attachMBFClient(std::shared_ptr mbfclient) 82 | { 83 | mbfclient_ = mbfclient; 84 | } 85 | 86 | BT::NodeStatus tick() override 87 | { 88 | if (mbfclient_) 89 | { 90 | ROS_INFO_STREAM("BT: " << this->name()); 91 | return (mbfclient_->prev_move() == State::MOVING) ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE; 92 | } 93 | 94 | return BT::NodeStatus::FAILURE; 95 | } 96 | 97 | private: 98 | std::shared_ptr mbfclient_; 99 | }; 100 | 101 | int main(int argc, char** argv) 102 | { 103 | ros::init(argc, argv, "behavior_tree"); 104 | ros::NodeHandle n; 105 | 106 | auto mbfclient = std::make_shared(std::move(mbf_advanced::loadPoseGoals(POSE_PATH))); 107 | 108 | BT::BehaviorTreeFactory factory; 109 | factory.registerSimpleCondition("DriveHomeStart", std::bind(DriveHome, std::ref(mbfclient))); 110 | factory.registerNodeType("AttemptNext"); 111 | factory.registerNodeType("AttemptSkip"); 112 | factory.registerNodeType("AttemptPrevious"); 113 | factory.registerNodeType("AttemptSkipPrevious"); 114 | factory.registerSimpleCondition("DriveHomeEnd", std::bind(DriveHome, std::ref(mbfclient))); 115 | 116 | auto tree = factory.createTreeFromFile(BT_XML_PATH); 117 | 118 | for( auto& node: tree.nodes) 119 | { 120 | if( auto attempt_next = dynamic_cast( node.get() )) 121 | { 122 | attempt_next->attachMBFClient(mbfclient); 123 | } 124 | 125 | if( auto attempt_skip = dynamic_cast( node.get() )) 126 | { 127 | attempt_skip->attachMBFClient(mbfclient); 128 | } 129 | 130 | if( auto attempt_prev = dynamic_cast( node.get() )) 131 | { 132 | attempt_prev->attachMBFClient(mbfclient); 133 | } 134 | } 135 | 136 | tree.tickRoot(); 137 | 138 | return 0; 139 | } -------------------------------------------------------------------------------- /advanced/src/mbf_client_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | int main(int argc, char** argv) 6 | { 7 | ros::init(argc, argv, "mbf_cpp_client_node"); 8 | ros::NodeHandle n; 9 | 10 | mbf_advanced::MBFCircleClient mbfclient(std::move(mbf_advanced::loadPoseGoals(POSE_PATH))); 11 | mbfclient.performCircle(); 12 | 13 | ROS_INFO_STREAM("Success"); 14 | } -------------------------------------------------------------------------------- /advanced/src/test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | BT::NodeStatus start() 4 | { 5 | std::cout << "start" << std::endl; 6 | return BT::NodeStatus::SUCCESS; 7 | } 8 | 9 | BT::NodeStatus stop() 10 | { 11 | std::cout << "stop" << std::endl; 12 | return BT::NodeStatus::FAILURE; 13 | } 14 | 15 | BT::NodeStatus final_stop() 16 | { 17 | std::cout << "final_stop" << std::endl; 18 | return BT::NodeStatus::SUCCESS; 19 | } 20 | 21 | //class Repeat: BT::SimpleActionNode 22 | //{ 23 | // Repeat(const std::string& name, const BT::NodeConfiguration &config) 24 | // : BT::RepeatNode(name, config) 25 | // {} 26 | // 27 | // static BT::PortsList providedPorts() 28 | // { 29 | // return { BT::InputPort("n_tries")}; 30 | // } 31 | // 32 | // BT::NodeStatus tick() override 33 | // { 34 | // BT::Optional msg = getInput("n_tries"); 35 | // if (!msg) 36 | // { 37 | // throw BT::RuntimeError("missing required input [n_tries]: ", 38 | // msg.error() ); 39 | // } 40 | // 41 | // // use the method value() to extract the valid message. 42 | // std::cout << "Robot says: " << msg.value() << std::endl; 43 | // return BT::NodeStatus::SUCCESS; 44 | // } 45 | //}; 46 | 47 | int main(int argc, char** argv) 48 | { 49 | BT::BehaviorTreeFactory factory; 50 | factory.registerSimpleCondition("start", std::bind(start)); 51 | factory.registerSimpleCondition("stop", std::bind(stop)); 52 | // factory.registerNodeType("Repeat"); 53 | factory.registerSimpleCondition("final_stop", std::bind(final_stop)); 54 | 55 | auto tree = factory.createTreeFromFile(BT_XML_PATH); 56 | tree.tickRoot(); 57 | 58 | return 0; 59 | } -------------------------------------------------------------------------------- /beginner/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(mbf_beginner) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | ################################### 7 | ## catkin specific configuration ## 8 | ################################### 9 | 10 | catkin_package() 11 | 12 | catkin_install_python( 13 | PROGRAMS scripts/mb_relay.py 14 | scripts/mb_relay_client.py 15 | scripts/mb_relay_server.py 16 | scripts/mb_relay_subscriber.py 17 | scripts/mbf_goal_client.py 18 | scripts/mbf_path_client.py 19 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 20 | ) 21 | -------------------------------------------------------------------------------- /beginner/TODO.md: -------------------------------------------------------------------------------- 1 | # TODO 2 | 3 | ## Misc 4 | - [ ] Credit rayman 5 | - [x] cancel_goals on shutdown hook 6 | - [ ] Circular Path with SimpleActionClient ((w/o) Relay) 7 | 8 | ## FAQ 9 | 10 | #### Planner Errors 11 | ``` 12 | [FATAL] [/move_base_flex]: Failed to load the base_local_planner/TrajectoryPlanner controller, are you sure it's properly registered and that the containing library is built? According to the loaded plugin descriptions the class base_local_planner/TrajectoryPlanner with base class type mbf_costmap_core::CostmapController does not exist. Declared types are According to the loaded plugin descriptions the class base_local_planner/TrajectoryPlanner with base class type nav_core::BaseLocalPlanner does not exist. Declared types are base_local_planner/TrajectoryPlannerROS dwa_local_planner/DWAPlannerROS 13 | [ERROR] [/move_base_flex]: Could not load the plugin with the name "base_local_planner/TrajectoryPlanner" and the type "base_local_planner/TrajectoryPlanner"! 14 | ``` 15 | 16 | 17 | -------------------------------------------------------------------------------- /beginner/launch/amcl_demo_base.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 | -------------------------------------------------------------------------------- /beginner/launch/amcl_demo_mbf.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /beginner/launch/amcl_demo_relay_server.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /beginner/launch/amcl_demo_relay_subscriber.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /beginner/launch/mb_relay_server.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /beginner/launch/mb_relay_subscriber.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /beginner/launch/mbf.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /beginner/launch/rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | -------------------------------------------------------------------------------- /beginner/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mbf_beginner 4 | 0.0.0 5 | Minimal example for move base flex and turtlebot 6 | 7 | Julian Gaal 8 | 9 | TODO 10 | 11 | catkin 12 | turtlebot3_navigation 13 | turtlebot3_gazebo 14 | dwa_local_planner 15 | eband_local_planner 16 | move_base_flex 17 | 18 | -------------------------------------------------------------------------------- /beginner/param/move_base_flex.yaml: -------------------------------------------------------------------------------- 1 | planners: 2 | - name: navfn/NavfnROS 3 | type: navfn/NavfnROS 4 | 5 | controllers: 6 | - name: eband_local_planner/EBandPlannerROS 7 | type: eband_local_planner/EBandPlannerROS 8 | 9 | controller_frequency: 5.0 10 | controller_patience: 3.0 11 | 12 | planner_frequency: 1.0 13 | planner_patience: 5.0 14 | 15 | recovery_enabled: false 16 | 17 | oscillation_timeout: 10.0 18 | oscillation_distance: 0.2 19 | -------------------------------------------------------------------------------- /beginner/param/turtlebot3_navigation.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Planner Path1 8 | - /Path1/Status1 9 | Splitter Ratio: 0.5 10 | Tree Height: 1172 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | Name: Tool Properties 18 | Splitter Ratio: 0.5886790156364441 19 | - Class: rviz/Views 20 | Expanded: 21 | - /Current View1 22 | Name: Views 23 | Splitter Ratio: 0.5 24 | - Class: rviz/Time 25 | Experimental: false 26 | Name: Time 27 | SyncMode: 0 28 | SyncSource: LaserScan 29 | Preferences: 30 | PromptSaveOnExit: true 31 | Toolbars: 32 | toolButtonStyle: 2 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.029999999329447746 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: 20 52 | Reference Frame: 53 | Value: true 54 | - Alpha: 1 55 | Class: rviz/RobotModel 56 | Collision Enabled: false 57 | Enabled: true 58 | Links: 59 | All Links Enabled: true 60 | Expand Joint Details: false 61 | Expand Link Details: false 62 | Expand Tree: false 63 | Link Tree Style: Links in Alphabetic Order 64 | base_footprint: 65 | Alpha: 1 66 | Show Axes: false 67 | Show Trail: false 68 | base_link: 69 | Alpha: 1 70 | Show Axes: false 71 | Show Trail: false 72 | Value: true 73 | base_scan: 74 | Alpha: 1 75 | Show Axes: false 76 | Show Trail: false 77 | Value: true 78 | caster_back_link: 79 | Alpha: 1 80 | Show Axes: false 81 | Show Trail: false 82 | Value: true 83 | imu_link: 84 | Alpha: 1 85 | Show Axes: false 86 | Show Trail: false 87 | wheel_left_link: 88 | Alpha: 1 89 | Show Axes: false 90 | Show Trail: false 91 | Value: true 92 | wheel_right_link: 93 | Alpha: 1 94 | Show Axes: false 95 | Show Trail: false 96 | Value: true 97 | Name: RobotModel 98 | Robot Description: robot_description 99 | TF Prefix: "" 100 | Update Interval: 0 101 | Value: true 102 | Visual Enabled: true 103 | - Alpha: 1 104 | Autocompute Intensity Bounds: true 105 | Autocompute Value Bounds: 106 | Max Value: 10 107 | Min Value: -10 108 | Value: true 109 | Axis: Z 110 | Channel Name: intensity 111 | Class: rviz/LaserScan 112 | Color: 0; 255; 0 113 | Color Transformer: FlatColor 114 | Decay Time: 0 115 | Enabled: true 116 | Invert Rainbow: false 117 | Max Color: 255; 255; 255 118 | Max Intensity: 13069 119 | Min Color: 0; 0; 0 120 | Min Intensity: 28 121 | Name: LaserScan 122 | Position Transformer: XYZ 123 | Queue Size: 10 124 | Selectable: true 125 | Size (Pixels): 3 126 | Size (m): 0.030000001192092896 127 | Style: Flat Squares 128 | Topic: /scan 129 | Unreliable: false 130 | Use Fixed Frame: true 131 | Use rainbow: true 132 | Value: true 133 | - Alpha: 0.699999988079071 134 | Class: rviz/Map 135 | Color Scheme: map 136 | Draw Behind: false 137 | Enabled: true 138 | Name: Map 139 | Topic: /map 140 | Unreliable: false 141 | Use Timestamp: false 142 | Value: true 143 | - Class: rviz/Group 144 | Displays: 145 | - Alpha: 0.699999988079071 146 | Class: rviz/Map 147 | Color Scheme: costmap 148 | Draw Behind: true 149 | Enabled: true 150 | Name: Costmap 151 | Topic: /move_base_flex/global_costmap/costmap 152 | Unreliable: false 153 | Use Timestamp: false 154 | Value: true 155 | - Alpha: 1 156 | Buffer Length: 1 157 | Class: rviz/Path 158 | Color: 255; 0; 0 159 | Enabled: true 160 | Head Diameter: 0.30000001192092896 161 | Head Length: 0.20000000298023224 162 | Length: 0.30000001192092896 163 | Line Style: Lines 164 | Line Width: 0.029999999329447746 165 | Name: Planner 166 | Offset: 167 | X: 0 168 | Y: 0 169 | Z: 0 170 | Pose Color: 255; 85; 255 171 | Pose Style: None 172 | Queue Size: 10 173 | Radius: 0.029999999329447746 174 | Shaft Diameter: 0.10000000149011612 175 | Shaft Length: 0.10000000149011612 176 | Topic: /move_base_flex/NavfnROS/plan 177 | Unreliable: false 178 | Value: true 179 | Enabled: true 180 | Name: Global Map 181 | - Class: rviz/Group 182 | Displays: 183 | - Alpha: 1 184 | Class: rviz/Polygon 185 | Color: 0; 0; 0 186 | Enabled: true 187 | Name: Polygon 188 | Queue Size: 10 189 | Topic: /move_base_flex/local_costmap/footprint 190 | Unreliable: false 191 | Value: true 192 | - Alpha: 0.699999988079071 193 | Class: rviz/Map 194 | Color Scheme: costmap 195 | Draw Behind: false 196 | Enabled: true 197 | Name: Costmap 198 | Topic: /move_base_flex/local_costmap/costmap 199 | Unreliable: false 200 | Use Timestamp: false 201 | Value: true 202 | - Alpha: 1 203 | Buffer Length: 1 204 | Class: rviz/Path 205 | Color: 255; 255; 0 206 | Enabled: true 207 | Head Diameter: 0.30000001192092896 208 | Head Length: 0.20000000298023224 209 | Length: 0.30000001192092896 210 | Line Style: Lines 211 | Line Width: 0.029999999329447746 212 | Name: Planner 213 | Offset: 214 | X: 0 215 | Y: 0 216 | Z: 0 217 | Pose Color: 255; 85; 255 218 | Pose Style: None 219 | Queue Size: 10 220 | Radius: 0.029999999329447746 221 | Shaft Diameter: 0.10000000149011612 222 | Shaft Length: 0.10000000149011612 223 | Topic: /move_base_flex/DWAPlannerROS/local_plan 224 | Unreliable: false 225 | Value: true 226 | Enabled: true 227 | Name: Local Map 228 | - Alpha: 1 229 | Arrow Length: 0.05000000074505806 230 | Axes Length: 0.30000001192092896 231 | Axes Radius: 0.009999999776482582 232 | Class: rviz/PoseArray 233 | Color: 0; 192; 0 234 | Enabled: true 235 | Head Length: 0.07000000029802322 236 | Head Radius: 0.029999999329447746 237 | Name: Amcl Particles 238 | Queue Size: 10 239 | Shaft Length: 0.23000000417232513 240 | Shaft Radius: 0.009999999776482582 241 | Shape: Arrow (Flat) 242 | Topic: /particlecloud 243 | Unreliable: false 244 | Value: true 245 | - Alpha: 1 246 | Buffer Length: 1 247 | Class: rviz/Path 248 | Color: 255; 0; 0 249 | Enabled: true 250 | Head Diameter: 0.30000001192092896 251 | Head Length: 0.20000000298023224 252 | Length: 0.30000001192092896 253 | Line Style: Billboards 254 | Line Width: 0.029999999329447746 255 | Name: Planner Path 256 | Offset: 257 | X: 0 258 | Y: 0 259 | Z: 0 260 | Pose Color: 255; 85; 255 261 | Pose Style: None 262 | Queue Size: 10 263 | Radius: 0.029999999329447746 264 | Shaft Diameter: 0.10000000149011612 265 | Shaft Length: 0.10000000149011612 266 | Topic: /move_base_flex/navfn/NavfnROS/plan 267 | Unreliable: false 268 | Value: true 269 | - Alpha: 1 270 | Axes Length: 1 271 | Axes Radius: 0.10000000149011612 272 | Class: rviz/Pose 273 | Color: 255; 25; 0 274 | Enabled: true 275 | Head Length: 0.10000000149011612 276 | Head Radius: 0.20000000298023224 277 | Name: Target Pose 278 | Queue Size: 10 279 | Shaft Length: 0.30000001192092896 280 | Shaft Radius: 0.10000000149011612 281 | Shape: Arrow 282 | Topic: /move_base_flex/current_goal 283 | Unreliable: false 284 | Value: true 285 | - Alpha: 1 286 | Buffer Length: 1 287 | Class: rviz/Path 288 | Color: 25; 255; 0 289 | Enabled: true 290 | Head Diameter: 0.30000001192092896 291 | Head Length: 0.20000000298023224 292 | Length: 0.30000001192092896 293 | Line Style: Lines 294 | Line Width: 0.029999999329447746 295 | Name: Path 296 | Offset: 297 | X: 0 298 | Y: 0 299 | Z: 0 300 | Pose Color: 255; 85; 255 301 | Pose Style: None 302 | Queue Size: 10 303 | Radius: 0.029999999329447746 304 | Shaft Diameter: 0.10000000149011612 305 | Shaft Length: 0.10000000149011612 306 | Topic: /move_base_flex/eband_local_planner/EBandPlannerROS/local_plan 307 | Unreliable: false 308 | Value: true 309 | - Alpha: 1 310 | Buffer Length: 1 311 | Class: rviz/Path 312 | Color: 25; 255; 0 313 | Enabled: true 314 | Head Diameter: 0.30000001192092896 315 | Head Length: 0.20000000298023224 316 | Length: 0.30000001192092896 317 | Line Style: Lines 318 | Line Width: 0.029999999329447746 319 | Name: Path 320 | Offset: 321 | X: 0 322 | Y: 0 323 | Z: 0 324 | Pose Color: 255; 85; 255 325 | Pose Style: None 326 | Queue Size: 10 327 | Radius: 0.029999999329447746 328 | Shaft Diameter: 0.10000000149011612 329 | Shaft Length: 0.10000000149011612 330 | Topic: /move_base_flex/eband_local_planner/EBandPlannerROS/global_plan 331 | Unreliable: false 332 | Value: true 333 | Enabled: true 334 | Global Options: 335 | Background Color: 48; 48; 48 336 | Default Light: true 337 | Fixed Frame: map 338 | Frame Rate: 30 339 | Name: root 340 | Tools: 341 | - Class: rviz/MoveCamera 342 | - Class: rviz/Interact 343 | Hide Inactive Objects: true 344 | - Class: rviz/Select 345 | - Class: rviz/SetInitialPose 346 | Theta std deviation: 0.2617993950843811 347 | Topic: /initialpose 348 | X std deviation: 0.5 349 | Y std deviation: 0.5 350 | - Class: rviz/SetGoal 351 | Topic: /move_base_simple/goal 352 | - Class: rviz/Measure 353 | Value: true 354 | Views: 355 | Current: 356 | Angle: -4.6707916259765625 357 | Class: rviz/TopDownOrtho 358 | Enable Stereo Rendering: 359 | Stereo Eye Separation: 0.05999999865889549 360 | Stereo Focal Distance: 1 361 | Swap Stereo Eyes: false 362 | Value: false 363 | Invert Z Axis: false 364 | Name: Current View 365 | Near Clip Distance: 0.009999999776482582 366 | Scale: -126.7267837524414 367 | Target Frame: 368 | X: -0.30647900700569153 369 | Y: -0.2619345784187317 370 | Saved: ~ 371 | Window Geometry: 372 | Displays: 373 | collapsed: false 374 | Height: 1404 375 | Hide Left Dock: false 376 | Hide Right Dock: true 377 | QMainWindow State: 000000ff00000000fd0000000400000000000001a100000520fc0200000007fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003e00000520000000ca00fffffffb0000000a0049006d0061006700650000000317000000cc0000000000000000fb0000000a0049006d0061006700650000000330000000ce0000000000000000000000010000010f000003a0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000043000003a0000000a600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002b900fffffffb0000000800540069006d006501000000000000045000000000000000000000034f0000052000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 378 | Selection: 379 | collapsed: false 380 | Time: 381 | collapsed: false 382 | Tool Properties: 383 | collapsed: false 384 | Views: 385 | collapsed: true 386 | Width: 1270 387 | X: 1282 388 | Y: 4 389 | -------------------------------------------------------------------------------- /beginner/scripts/mb_relay.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # @author Jorge Santos 4 | # License: 3-Clause BSD 5 | # TODO how to credit 6 | 7 | import actionlib 8 | import copy 9 | 10 | import rospy 11 | import nav_msgs.srv as nav_srvs 12 | import mbf_msgs.msg as mbf_msgs 13 | import move_base_msgs.msg as mb_msgs 14 | from dynamic_reconfigure.client import Client 15 | from geometry_msgs.msg import PoseStamped 16 | from move_base.cfg import MoveBaseConfig 17 | 18 | # Navigation server loads simply what was configured when left empty 19 | # https://github.com/magazino/move_base_flex/blob/a8fb229161007a08c705d510765b37ce8e57cb55/mbf_abstract_nav/src/abstract_navigation_server.cpp#L130-L133 20 | bgp = None 21 | blp = None 22 | 23 | 24 | def simple_goal_cb(msg): 25 | mbf_mb_ac.send_goal(mbf_msgs.MoveBaseGoal(target_pose=msg, planner=bgp, controller=blp)) 26 | rospy.logdebug("Relaying move_base_simple/goal pose to mbf") 27 | 28 | 29 | def mb_execute_cb(msg): 30 | mbf_mb_ac.send_goal(mbf_msgs.MoveBaseGoal(target_pose=msg.target_pose, planner=bgp, controller=blp), 31 | feedback_cb=mbf_feedback_cb) 32 | rospy.logdebug("Relaying move_base goal to mbf") 33 | mbf_mb_ac.wait_for_result() 34 | 35 | status = mbf_mb_ac.get_state() 36 | result = mbf_mb_ac.get_result() 37 | 38 | rospy.logdebug("MBF execution completed with result [%d]: %s", result.outcome, result.message) 39 | if result.outcome == mbf_msgs.MoveBaseResult.SUCCESS: 40 | mb_as.set_succeeded(mb_msgs.MoveBaseResult(), "Goal reached.") 41 | else: 42 | mb_as.set_aborted(mb_msgs.MoveBaseResult(), result.message) 43 | 44 | 45 | def make_plan_cb(request): 46 | mbf_gp_ac.send_goal(mbf_msgs.GetPathGoal(start_pose=request.start, target_pose=request.goal, 47 | use_start_pose=bool(request.start.header.frame_id), 48 | planner=bgp, tolerance=request.tolerance)) 49 | rospy.logdebug("Relaying legacy make_plan service to mbf get_path action server") 50 | mbf_gp_ac.wait_for_result() 51 | 52 | status = mbf_gp_ac.get_state() 53 | result = mbf_gp_ac.get_result() 54 | 55 | rospy.logdebug("MBF get_path execution completed with result [%d]: %s", result.outcome, result.message) 56 | if result.outcome == mbf_msgs.GetPathResult.SUCCESS: 57 | return nav_srvs.GetPlanResponse(plan=result.path) 58 | 59 | 60 | def mbf_feedback_cb(feedback): 61 | mb_as.publish_feedback(mb_msgs.MoveBaseFeedback(base_position=feedback.current_pose)) 62 | 63 | 64 | if __name__ == '__main__': 65 | rospy.init_node("move_base") 66 | 67 | # move_base_flex get_path and move_base action clients 68 | mbf_mb_ac = actionlib.SimpleActionClient("move_base_flex/move_base", mbf_msgs.MoveBaseAction) 69 | mbf_gp_ac = actionlib.SimpleActionClient("move_base_flex/get_path", mbf_msgs.GetPathAction) 70 | mbf_mb_ac.wait_for_server(rospy.Duration(20)) 71 | mbf_gp_ac.wait_for_server(rospy.Duration(10)) 72 | 73 | 74 | # move_base simple topic and action server 75 | mb_sg = rospy.Subscriber('move_base_simple/goal', PoseStamped, simple_goal_cb) 76 | mb_as = actionlib.SimpleActionServer('move_base', mb_msgs.MoveBaseAction, mb_execute_cb, auto_start=False) 77 | mb_as.start() 78 | 79 | # move_base make_plan service 80 | mb_mps = rospy.Service('~make_plan', nav_srvs.GetPlan, make_plan_cb) 81 | 82 | rospy.spin() 83 | -------------------------------------------------------------------------------- /beginner/scripts/mb_relay_client.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | import rospy 4 | import actionlib 5 | import mbf_msgs.msg as mbf_msgs 6 | import move_base_msgs.msg as mb_msgs 7 | from actionlib_msgs.msg import GoalStatus 8 | 9 | def create_goal(x, y, z, xx, yy, zz, ww): 10 | goal = mb_msgs.MoveBaseGoal() 11 | goal.target_pose.header.frame_id = "map" 12 | goal.target_pose.header.stamp = rospy.Time.now() 13 | goal.target_pose.pose.position.x = x 14 | goal.target_pose.pose.position.y = y 15 | goal.target_pose.pose.position.z = z 16 | goal.target_pose.pose.orientation.x = xx 17 | goal.target_pose.pose.orientation.y = yy 18 | goal.target_pose.pose.orientation.z = zz 19 | goal.target_pose.pose.orientation.w = ww 20 | return goal 21 | 22 | def move(goal): 23 | client.send_goal(goal) 24 | client.wait_for_result() 25 | return client.get_state() == GoalStatus.SUCCEEDED 26 | 27 | 28 | def drive_circle(): 29 | goals = [ create_goal(-1.75, 0.74, 0, 0, 0, 0.539, 0.843), 30 | create_goal(-0.36, 1.92, 0, 0, 0, -0.020, 0.999), 31 | create_goal(0.957, 1.60, 0, 0, 0, -0.163, 0.987), 32 | create_goal(1.8741, 0.3830, 0, 0, 0, -0.70, 0.711), 33 | create_goal(1.752, -0.928, 0, 0, 0, -0.856, 0.517), 34 | create_goal(0.418, -2.116, 0, 0, 0, 0.998, 0.0619), 35 | create_goal(-0.775, -1.80, 0, 0, 0, 0.954, 0.300), 36 | create_goal(-1.990, -0.508, 0, 0, 0, -0.112, 0.999) 37 | ] 38 | 39 | for goal in goals: 40 | rospy.loginfo("Attempting to drive to %s %s", goal.target_pose.pose.position.x, goal.target_pose.pose.position.y) 41 | if not move(goal): 42 | return False 43 | 44 | return True 45 | 46 | if __name__ == '__main__': 47 | try: 48 | rospy.init_node('mb_relay_client') 49 | 50 | client = actionlib.SimpleActionClient('move_base', mb_msgs.MoveBaseAction) 51 | client.wait_for_server(rospy.Duration(10)) 52 | rospy.loginfo("Connected to SimpleActionServer 'move_base'") 53 | 54 | result = drive_circle() 55 | rospy.loginfo("Drove circle with result: %s", result) 56 | 57 | except rospy.ROSInterruptException: 58 | rospy.logerror("program interrupted before completion") -------------------------------------------------------------------------------- /beginner/scripts/mb_relay_server.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # @author Jorge Santos 4 | # License: 3-Clause BSD 5 | # TODO how to credit 6 | 7 | import actionlib 8 | import rospy 9 | import mbf_msgs.msg as mbf_msgs 10 | import move_base_msgs.msg as mb_msgs 11 | 12 | def mb_execute_cb(msg): 13 | mbf_mb_ac.send_goal(mbf_msgs.MoveBaseGoal(target_pose=msg.target_pose), 14 | feedback_cb=mbf_feedback_cb) 15 | 16 | rospy.logdebug("Relaying move_base goal to mbf") 17 | mbf_mb_ac.wait_for_result() 18 | 19 | status = mbf_mb_ac.get_state() 20 | result = mbf_mb_ac.get_result() 21 | 22 | rospy.logdebug("MBF execution completed with result [%d]: %s", result.outcome, result.message) 23 | if result.outcome == mbf_msgs.MoveBaseResult.SUCCESS: 24 | mb_as.set_succeeded(mb_msgs.MoveBaseResult(), "Goal reached.") 25 | else: 26 | mb_as.set_aborted(mb_msgs.MoveBaseResult(), result.message) 27 | 28 | def mbf_feedback_cb(feedback): 29 | mb_as.publish_feedback(mb_msgs.MoveBaseFeedback(base_position=feedback.current_pose)) 30 | 31 | if __name__ == '__main__': 32 | rospy.init_node("move_base") 33 | 34 | # move_base_flex get_path and move_base action clients 35 | mbf_mb_ac = actionlib.SimpleActionClient("move_base_flex/move_base", mbf_msgs.MoveBaseAction) 36 | mbf_mb_ac.wait_for_server(rospy.Duration(10)) 37 | 38 | mb_as = actionlib.SimpleActionServer('move_base', mb_msgs.MoveBaseAction, mb_execute_cb, auto_start=False) 39 | mb_as.start() 40 | 41 | rospy.spin() 42 | -------------------------------------------------------------------------------- /beginner/scripts/mb_relay_subscriber.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # @author Jorge Santos 4 | # License: 3-Clause BSD 5 | # TODO how to credit 6 | 7 | import actionlib 8 | import rospy 9 | import nav_msgs.srv as nav_srvs 10 | import mbf_msgs.msg as mbf_msgs 11 | import move_base_msgs.msg as mb_msgs 12 | from geometry_msgs.msg import PoseStamped 13 | 14 | 15 | def simple_goal_cb(msg): 16 | mbf_mb_ac.send_goal(mbf_msgs.MoveBaseGoal(target_pose=msg)) 17 | rospy.logdebug("Relaying move_base_simple/goal pose to mbf") 18 | 19 | mbf_mb_ac.wait_for_result() 20 | 21 | status = mbf_mb_ac.get_state() 22 | result = mbf_mb_ac.get_result() 23 | 24 | rospy.logdebug("MBF execution completed with result [%d]: %s", result.outcome, result.message) 25 | 26 | if __name__ == '__main__': 27 | rospy.init_node("move_base_relay") 28 | 29 | # move base flex ation client relays incoming mb goals to mbf 30 | mbf_mb_ac = actionlib.SimpleActionClient("move_base_flex/move_base", mbf_msgs.MoveBaseAction) 31 | mbf_mb_ac.wait_for_server(rospy.Duration(20)) 32 | 33 | # move_base simple topic and action server 34 | mb_sg = rospy.Subscriber('move_base_simple/goal', PoseStamped, simple_goal_cb) 35 | 36 | rospy.on_shutdown(lambda: mbf_mb_ac.cancel_all_goals()) 37 | 38 | rospy.spin() -------------------------------------------------------------------------------- /beginner/scripts/mbf_goal_client.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # @author Julian Gaal 4 | # License: 3-Clause BSD 5 | 6 | import actionlib 7 | import rospy 8 | import mbf_msgs.msg as mbf_msgs 9 | 10 | 11 | def create_goal(x, y, z, xx, yy, zz, ww): 12 | goal = mbf_msgs.MoveBaseGoal() 13 | goal.target_pose.header.frame_id = "map" 14 | goal.target_pose.header.stamp = rospy.Time.now() 15 | goal.target_pose.pose.position.x = x 16 | goal.target_pose.pose.position.y = y 17 | goal.target_pose.pose.position.z = z 18 | goal.target_pose.pose.orientation.x = xx 19 | goal.target_pose.pose.orientation.y = yy 20 | goal.target_pose.pose.orientation.z = zz 21 | goal.target_pose.pose.orientation.w = ww 22 | return goal 23 | 24 | 25 | def move(goal): 26 | mbf_ac.send_goal(goal) 27 | mbf_ac.wait_for_result() 28 | return mbf_ac.get_result() 29 | 30 | 31 | def drive_circle(): 32 | goals = [ create_goal(-1.75, 0.74, 0, 0, 0, 0.539, 0.843), 33 | create_goal(-0.36, 1.92, 0, 0, 0, -0.020, 0.999), 34 | create_goal(0.957, 1.60, 0, 0, 0, -0.163, 0.987), 35 | create_goal(1.8741, 0.3830, 0, 0, 0, -0.70, 0.711), 36 | create_goal(1.752, -0.928, 0, 0, 0, -0.856, 0.517), 37 | create_goal(0.418, -2.116, 0, 0, 0, 0.998, 0.0619), 38 | create_goal(-0.775, -1.80, 0, 0, 0, 0.954, 0.300), 39 | create_goal(-1.990, -0.508, 0, 0, 0, -0.100, 0.999) 40 | ] 41 | 42 | for goal in goals: 43 | rospy.loginfo("Attempting to reach (%1.3f, %1.3f)", goal.target_pose.pose.position.x, goal.target_pose.pose.position.y) 44 | result = move(goal) 45 | 46 | if result.outcome != mbf_msgs.MoveBaseResult.SUCCESS: 47 | rospy.loginfo("Unable to complete action: %s", result.message) 48 | return 49 | 50 | if __name__ == '__main__': 51 | rospy.init_node("move_base_flex_client") 52 | 53 | # move_base_flex action client 54 | mbf_ac = actionlib.SimpleActionClient("move_base_flex/move_base", mbf_msgs.MoveBaseAction) 55 | mbf_ac.wait_for_server(rospy.Duration(10)) 56 | rospy.loginfo("Connected to Move Base Flex action server!") 57 | 58 | drive_circle() 59 | 60 | rospy.on_shutdown(lambda: mbf_ac.cancel_all_goals()) 61 | 62 | -------------------------------------------------------------------------------- /beginner/scripts/mbf_path_client.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # @author Julian Gaal 4 | # License: 3-Clause BSD 5 | 6 | import actionlib 7 | import rospy 8 | import mbf_msgs.msg as mbf_msgs 9 | import geometry_msgs.msg as geometry_msgs 10 | 11 | 12 | def create_pose(x, y, z, xx, yy, zz, ww): 13 | pose = geometry_msgs.PoseStamped() 14 | pose.header.frame_id = "map" 15 | pose.header.stamp = rospy.Time.now() 16 | pose.pose.position.x = x 17 | pose.pose.position.y = y 18 | pose.pose.position.z = z 19 | pose.pose.orientation.x = xx 20 | pose.pose.orientation.y = yy 21 | pose.pose.orientation.z = zz 22 | pose.pose.orientation.w = ww 23 | return pose 24 | 25 | 26 | def create_path_goal(path, tolerance_from_action, dist_tolerance, angle_tolerance): 27 | goal = mbf_msgs.ExePathGoal() 28 | goal.path = path 29 | goal.tolerance_from_action = tolerance_from_action 30 | goal.dist_tolerance = dist_tolerance 31 | goal.angle_tolerance = angle_tolerance 32 | return goal 33 | 34 | 35 | def exe_path(path_goal): 36 | mbf_ep_ac.send_goal(path_goal) 37 | mbf_ep_ac.wait_for_result() 38 | return mbf_ep_ac.get_result() 39 | 40 | 41 | def get_plan(pose): 42 | path_goal = mbf_msgs.GetPathGoal(target_pose=pose, tolerance=0.5) 43 | mbf_gp_ac.send_goal(path_goal) 44 | mbf_gp_ac.wait_for_result() 45 | return mbf_gp_ac.get_result() 46 | 47 | 48 | def drive_circle(): 49 | target_poses = [ 50 | create_pose(-1.75, 0.74, 0, 0, 0, 0.539, 0.843), 51 | create_pose(-0.36, 1.92, 0, 0, 0, -0.020, 0.999), 52 | create_pose(0.957, 1.60, 0, 0, 0, -0.163, 0.987), 53 | create_pose(1.8741, 0.3830, 0, 0, 0, -0.70, 0.711), 54 | create_pose(1.752, -0.928, 0, 0, 0, -0.856, 0.517), 55 | create_pose(0.418, -2.116, 0, 0, 0, 0.998, 0.0619), 56 | create_pose(-0.775, -1.80, 0, 0, 0, 0.954, 0.300), 57 | create_pose(-1.990, -0.508, 0, 0, 0, -0.112, 0.999) 58 | ] 59 | 60 | for target_pose in target_poses: 61 | rospy.loginfo("Attempting to reach (%1.3f, %1.3f)", target_pose.pose.position.x, target_pose.pose.position.y) 62 | 63 | get_path_result = get_plan(target_pose) 64 | if get_path_result.outcome != mbf_msgs.MoveBaseResult.SUCCESS: 65 | rospy.loginfo("Unable to complete plan: %s", result.message) 66 | return 67 | 68 | path_goal = create_path_goal(get_path_result.path, True, 0.5, 3.14/18.0) 69 | 70 | exe_path_result = exe_path(path_goal) 71 | if exe_path_result.outcome != mbf_msgs.MoveBaseResult.SUCCESS: 72 | rospy.loginfo("Unable to complete exe: %s", result.message) 73 | return 74 | 75 | 76 | if __name__ == '__main__': 77 | rospy.init_node("move_base_flex_client") 78 | 79 | # move_base_flex exe path client 80 | mbf_ep_ac = actionlib.SimpleActionClient("move_base_flex/exe_path", mbf_msgs.ExePathAction) 81 | mbf_ep_ac.wait_for_server(rospy.Duration(10)) 82 | rospy.loginfo("Connected to Move Base Flex ExePath server!") 83 | 84 | # move base flex get path client 85 | mbf_gp_ac = actionlib.SimpleActionClient("move_base_flex/get_path", mbf_msgs.GetPathAction) 86 | mbf_gp_ac.wait_for_server(rospy.Duration(10)) 87 | 88 | drive_circle() 89 | 90 | rospy.on_shutdown(lambda: mbf_ep_ac.cancel_all_goals()) 91 | 92 | -------------------------------------------------------------------------------- /demo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/naturerobots/mbf_tutorials/7a15ba8425fa354b6b6444eaa14c60f5d90915bb/demo.gif --------------------------------------------------------------------------------