BehaviourTree.CPP BT Tutorial extendend

juliangaal · juliangaal · commit 14b0a37aaaea · 2021-04-08T16:42:12.000+02:00
diff --git a/advanced/CMakeLists.txt b/advanced/CMakeLists.txt
@@ -21,10 +21,10 @@ catkin_install_python(
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 
-add_executable(mbf_cpp_client_node src/mbf_cpp_client_node.cpp)
-target_include_directories(mbf_cpp_client_node PRIVATE include ${catkin_INCLUDE_DIRS})
-target_link_libraries(mbf_cpp_client_node ${catkin_LIBRARIES})
-target_compile_definitions(mbf_cpp_client_node
+add_executable(mbf_client_node src/mbf_client_node.cpp)
+target_include_directories(mbf_client_node PRIVATE include ${catkin_INCLUDE_DIRS})
+target_link_libraries(mbf_client_node ${catkin_LIBRARIES})
+target_compile_definitions(mbf_client_node
         PRIVATE
         POSE_PATH="${PROJECT_SOURCE_DIR}/param/poses.txt"
 )
diff --git a/advanced/include/mbf_advanced/mbf_circle_client.h b/advanced/include/mbf_advanced/mbf_circle_client.h
@@ -10,14 +10,22 @@
 namespace mbf_advanced
 {
 
-struct MBFClient
+enum MBFCircleClientState
 {
-    explicit MBFClient(std::vector<mbf_msgs::MoveBaseGoal> pose_goals)
+    AT_END = 0,
+    MOVING,
+    FAILED
+};
+
+struct MBFCircleClient
+{
+    explicit MBFCircleClient(std::vector<mbf_msgs::MoveBaseGoal> pose_goals)
             : pose_goals_(std::move(pose_goals))
             , it_(pose_goals_.begin())
             , prev_move_(pose_goals_.begin())
             , home_(pose_goals_.back())
             , ac_("move_base_flex/move_base", true)
+            , terminate_(false)
     {
         ac_.waitForServer();
         ROS_INFO("Connected to MBF action server");
@@ -49,36 +57,43 @@ struct MBFClient
         return true;
     }
 
-    bool next_move()
+    MBFCircleClientState next_move()
     {
         if (at_end())
         {
-            throw std::runtime_error("Reached end! No more next poses. Exiting BT.");
+            ROS_INFO_STREAM("Reached end of circle");
+            terminate_ = true;
+            return MBFCircleClientState::AT_END;
         }
 
+        prev_move_ = it_-1;
         auto result = log_move(*it_);
-        prev_move_ = it_;
         ++it_;
-        return result;
+        return result ? MBFCircleClientState::MOVING : MBFCircleClientState::FAILED;
     }
 
     bool at_end()
     {
         return it_ == pose_goals_.end();
     }
 
-    bool prev_move()
-    {
+    MBFCircleClientState prev_move()
+    {   
+        if (terminate_)
+        {
+            return MBFCircleClientState::AT_END;
+        }
+
         if (prev_move_ != pose_goals_.begin())
         {
             auto result = log_move(*prev_move_);
             it_ = prev_move_+1;
-            prev_move_ = it_-1;
-            return result;
+            --prev_move_;
+            return result ? MBFCircleClientState::MOVING : MBFCircleClientState::FAILED;
         }
         else
         {
-            return driveHome();
+            return driveHome() ? MBFCircleClientState::MOVING : MBFCircleClientState::FAILED;
         }
     }
 
@@ -93,6 +108,7 @@ struct MBFClient
     std::vector<mbf_msgs::MoveBaseGoal>::const_iterator prev_move_;
     const mbf_msgs::MoveBaseGoal& home_;
     actionlib::SimpleActionClient<mbf_msgs::MoveBaseAction> ac_;
+    bool terminate_;
 };
 
 } // end
diff --git a/advanced/param/mbf_behavior_tree.xml b/advanced/param/mbf_behavior_tree.xml
@@ -2,13 +2,17 @@
     <BehaviorTree ID="MainTree">
         <Sequence name="endless_circle">
             <DriveHomeStart name="drive_home_start"/>
-                <Repeat num_cycles="-1">
-                <Fallback>
-                    <AttemptNext name="attempt_next"/>
-                    <AttemptPrevious name="attempt_previous"/>
-                    <DriveHomeEnd name="drive_home_end"/>
-                </Fallback>
-            </Repeat>
+                <ForceSuccess>
+                    <Repeat num_cycles="10">
+                        <Fallback>
+                            <AttemptNext name="attempt_next"/>
+                            <AttemptSkip name="attempt_skip"/>
+                            <AttemptPrevious name="attempt_previous"/>
+                            <AttemptSkipPrevious name="attempt_skip_previous"/>
+                        </Fallback>
+                    </Repeat>
+                </ForceSuccess>
+            <DriveHomeEnd name="drive_home_end"/>
         </Sequence>
     </BehaviorTree>
  </root>
diff --git a/advanced/scripts/circle_smach.py b/advanced/scripts/circle_smach.py
diff --git a/advanced/scripts/circle_smach_continuous.py b/advanced/scripts/circle_smach_continuous.py
@@ -0,0 +1,132 @@
+import rospy
+import smach
+import smach_ros
+import threading
+
+from geometry_msgs.msg import PoseStamped
+from nav_msgs.msg import Path
+
+from mbf_msgs.msg import ExePathAction
+from mbf_msgs.msg import GetPathAction
+from mbf_msgs.msg import RecoveryAction
+
+
+def create_pose(x, y, z, xx, yy, zz, ww):
+    pose = PoseStamped()
+    pose.header.frame_id = "map"
+    pose.header.stamp = rospy.Time.now()
+    pose.pose.position.x = x
+    pose.pose.position.y = y
+    pose.pose.position.z = z
+    pose.pose.orientation.x = xx
+    pose.pose.orientation.y = yy
+    pose.pose.orientation.z = zz
+    pose.pose.orientation.w = ww
+    return pose
+
+
+def iterate_target_poses():
+    target_poses = [   
+        create_pose(-1.75, 0.74, 0, 0, 0, 0.539, 0.843),
+        create_pose(-0.36, 1.92, 0, 0, 0, -0.020, 0.999),
+        create_pose(0.957, 1.60, 0, 0, 0, -0.163, 0.987),
+        create_pose(1.8741, 0.3830, 0, 0, 0, -0.70, 0.711),
+        create_pose(1.752, -0.928, 0, 0, 0, -0.856, 0.517),
+        create_pose(0.418, -2.116, 0, 0, 0, 0.998, 0.0619),
+        create_pose(-0.775, -1.80, 0, 0, 0, 0.954, 0.300),
+        create_pose(-1.990, -0.508, 0, 0, 0, -0.112, 0.999)
+    ]
+
+    for target_pose in target_poses:
+        yield target_pose
+
+def create_path_goal(path, tolerance_from_action, dist_tolerance, angle_tolerance):
+    goal = mbf_msgs.ExePathGoal()
+    goal.path = path
+    goal.tolerance_from_action = tolerance_from_action
+    goal.dist_tolerance = dist_tolerance
+    goal.angle_tolerance = angle_tolerance
+    return goal
+
+def main():
+    rospy.init_node('mbf_state_machine')
+
+    target_poses = iterate_target_poses()
+
+    # Create SMACH state machine
+    sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
+
+    # Define userdata
+    sm.userdata.target_pose = None
+    sm.userdata.path = None
+    sm.userdata.error = None
+    sm.userdata.clear_costmap_flag = False
+    sm.userdata.error_status = None
+
+    with sm:
+        # path callback
+        def get_path_callback(userdata, goal):
+            try:
+                goal.target_pose = next(target_poses)
+            except StopIteration:
+                rospy.logwarn("Reached last target pose")
+                rospy.signal_shutdown("Last goal reached. Shutting down")
+
+        # Get path
+        smach.StateMachine.add(
+            'GET_PATH',
+            smach_ros.SimpleActionState(
+                '/move_base_flex/get_path',
+                GetPathAction,
+                goal_cb=get_path_callback,
+                goal_slots=['target_pose'],
+                result_slots=['path']
+            ),
+            transitions={
+                'succeeded': 'EXE_PATH',
+                'aborted': 'FAILED',
+                'preempted': 'preempted'
+            }
+        )
+
+        def path_callback(userdata, goal):
+            target_pose = goal.path.poses[-1].pose
+            rospy.loginfo("Attempting to reach (%1.3f, %1.3f)", target_pose.position.x, target_pose.position.y)
+
+        # Execute path
+        smach.StateMachine.add(
+            'EXE_PATH',
+            smach_ros.SimpleActionState(
+                '/move_base_flex/exe_path',
+                ExePathAction,
+                goal_cb=path_callback,
+                goal_slots=['path']
+            ),
+            transitions={
+                'succeeded': 'GET_PATH',
+                'aborted': 'FAILED',
+                'preempted': 'preempted'
+            }
+        )
+
+        smach.StateMachine.add(
+            'FAILED',
+
+        )
+
+    # Execute SMACH plan
+    # Create a thread to execute the smach container
+    smach_thread = threading.Thread(target=sm.execute)
+    smach_thread.start()
+
+    # Wait for ctrl-c
+    rospy.spin()
+
+    # Request the container to preempt
+    sm.request_preempt()
+
+    # Block until everything is preempted 
+    smach_thread.join()
+
+if __name__=="__main__":
+    main()
diff --git a/advanced/src/mbf_behavior_tree.cpp b/advanced/src/mbf_behavior_tree.cpp
@@ -1,10 +1,11 @@
 #include <fstream>
 #include <ros/ros.h>
 #include <behaviortree_cpp_v3/bt_factory.h>
-#include <mbf_advanced/mbf_cpp_client.h>
+#include <mbf_advanced/mbf_circle_client.h>
 
+using State = mbf_advanced::MBFCircleClientState;
 
-BT::NodeStatus DriveHome(std::shared_ptr<mbf_advanced::MBFClient>& mbfclient)
+BT::NodeStatus DriveHome(std::shared_ptr<mbf_advanced::MBFCircleClient>& mbfclient)
 {
     ROS_INFO_STREAM("BT: driving home");
     return mbfclient->driveHome() ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE;
@@ -18,7 +19,7 @@ class AttemptNext : public BT::SyncActionNode
       , mbfclient_{}
     { }
 
-    void attachMBFClient(std::shared_ptr<mbf_advanced::MBFClient> mbfclient)
+    void attachMBFClient(std::shared_ptr<mbf_advanced::MBFCircleClient> mbfclient)
     {
         mbfclient_ = mbfclient;
     }
@@ -28,14 +29,45 @@ class AttemptNext : public BT::SyncActionNode
         if (mbfclient_)
         {
             ROS_INFO_STREAM("BT: " << this->name());
-            return mbfclient_->next_move() ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE;
+
+            while (mbfclient_->next_move() == State::MOVING) {}
+
+            return BT::NodeStatus::FAILURE;
+        }
+
+        return BT::NodeStatus::FAILURE;
+    }
+
+private:
+    std::shared_ptr<mbf_advanced::MBFCircleClient> mbfclient_;
+};
+
+class AttemptSkip : public BT::SyncActionNode
+{
+public:
+    explicit AttemptSkip(const std::string& name)
+      : BT::SyncActionNode(name, {})
+      , mbfclient_{}
+    { }
+
+    void attachMBFClient(std::shared_ptr<mbf_advanced::MBFCircleClient> mbfclient)
+    {
+        mbfclient_ = mbfclient;
+    }
+
+    BT::NodeStatus tick() override
+    {
+        if (mbfclient_)
+        {
+            ROS_INFO_STREAM("BT: " << this->name());
+            return (mbfclient_->next_move() == State::MOVING) ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE;
         }
 
         return BT::NodeStatus::FAILURE;
     }
 
 private:
-    std::shared_ptr<mbf_advanced::MBFClient> mbfclient_;
+    std::shared_ptr<mbf_advanced::MBFCircleClient> mbfclient_;
 };
 
 class AttemptPrevious : public BT::SyncActionNode
@@ -46,7 +78,7 @@ class AttemptPrevious : public BT::SyncActionNode
             , mbfclient_{}
     { }
 
-    void attachMBFClient(std::shared_ptr<mbf_advanced::MBFClient> mbfclient)
+    void attachMBFClient(std::shared_ptr<mbf_advanced::MBFCircleClient> mbfclient)
     {
         mbfclient_ = mbfclient;
     }
@@ -56,27 +88,29 @@ class AttemptPrevious : public BT::SyncActionNode
         if (mbfclient_)
         {
             ROS_INFO_STREAM("BT: " << this->name());
-            return mbfclient_->prev_move() ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE;
+            return (mbfclient_->prev_move() == State::MOVING) ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE;
         }
 
         return BT::NodeStatus::FAILURE;
     }
 
 private:
-    std::shared_ptr<mbf_advanced::MBFClient> mbfclient_;
+    std::shared_ptr<mbf_advanced::MBFCircleClient> mbfclient_;
 };
 
 int main(int argc, char** argv)
 {
     ros::init(argc, argv, "behavior_tree");
     ros::NodeHandle n;
 
-    auto mbfclient = std::make_shared<mbf_advanced::MBFClient>(std::move(mbf_advanced::loadPoseGoals(POSE_PATH)));
+    auto mbfclient = std::make_shared<mbf_advanced::MBFCircleClient>(std::move(mbf_advanced::loadPoseGoals(POSE_PATH)));
 
     BT::BehaviorTreeFactory factory;
     factory.registerSimpleCondition("DriveHomeStart", std::bind(DriveHome, std::ref(mbfclient)));
     factory.registerNodeType<AttemptNext>("AttemptNext");
+    factory.registerNodeType<AttemptSkip>("AttemptSkip");
     factory.registerNodeType<AttemptPrevious>("AttemptPrevious");
+    factory.registerNodeType<AttemptPrevious>("AttemptSkipPrevious");
     factory.registerSimpleCondition("DriveHomeEnd", std::bind(DriveHome, std::ref(mbfclient)));
 
     auto tree = factory.createTreeFromFile(BT_XML_PATH);
@@ -88,6 +122,11 @@ int main(int argc, char** argv)
             attempt_next->attachMBFClient(mbfclient);
         }
 
+        if( auto attempt_skip = dynamic_cast<AttemptSkip*>( node.get() ))
+        {
+            attempt_skip->attachMBFClient(mbfclient);
+        }
+
         if( auto attempt_prev = dynamic_cast<AttemptPrevious*>( node.get() ))
         {
             attempt_prev->attachMBFClient(mbfclient);
diff --git a/advanced/src/mbf_client_node.cpp b/advanced/src/mbf_client_node.cpp

Original file line number	Diff line number	Diff line change
`@@ -21,10 +21,10 @@ catkin_install_python(`
`21`	`21`	`DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}`
`22`	`22`	`)`
`23`	`23`
`24`		`-add_executable(mbf_cpp_client_node src/mbf_cpp_client_node.cpp)`
`25`		`-target_include_directories(mbf_cpp_client_node PRIVATE include ${catkin_INCLUDE_DIRS})`
`26`		`-target_link_libraries(mbf_cpp_client_node ${catkin_LIBRARIES})`
`27`		`-target_compile_definitions(mbf_cpp_client_node`
	`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`	`28`	`PRIVATE`
`29`	`29`	`POSE_PATH="${PROJECT_SOURCE_DIR}/param/poses.txt"`
`30`	`30`	`)`
Original file line number	Diff line number	Diff line change
`@@ -10,14 +10,22 @@`
`10`	`10`	`namespace mbf_advanced`
`11`	`11`	`{`
`12`	`12`
`13`		`-struct MBFClient`
	`13`	`+enum MBFCircleClientState`
`14`	`14`	`{`
`15`		`- explicit MBFClient(std::vector<mbf_msgs::MoveBaseGoal> pose_goals)`
	`15`	`+ AT_END = 0,`
	`16`	`+ MOVING,`
	`17`	`+ FAILED`
	`18`	`+};`
	`19`	`+`
	`20`	`+struct MBFCircleClient`
	`21`	`+{`
	`22`	`+ explicit MBFCircleClient(std::vector<mbf_msgs::MoveBaseGoal> pose_goals)`
`16`	`23`	`: pose_goals_(std::move(pose_goals))`
`17`	`24`	`, it_(pose_goals_.begin())`
`18`	`25`	`, prev_move_(pose_goals_.begin())`
`19`	`26`	`, home_(pose_goals_.back())`
`20`	`27`	`, ac_("move_base_flex/move_base", true)`
	`28`	`+ , terminate_(false)`
`21`	`29`	`{`
`22`	`30`	`ac_.waitForServer();`
`23`	`31`	`ROS_INFO("Connected to MBF action server");`
`@@ -49,36 +57,43 @@ struct MBFClient`
`49`	`57`	`return true;`
`50`	`58`	`}`
`51`	`59`
`52`		`- bool next_move()`
	`60`	`+ MBFCircleClientState next_move()`
`53`	`61`	`{`
`54`	`62`	`if (at_end())`
`55`	`63`	`{`
`56`		`- throw std::runtime_error("Reached end! No more next poses. Exiting BT.");`
	`64`	`+ ROS_INFO_STREAM("Reached end of circle");`
	`65`	`+ terminate_ = true;`
	`66`	`+ return MBFCircleClientState::AT_END;`
`57`	`67`	`}`
`58`	`68`
	`69`	`+ prev_move_ = it_-1;`
`59`	`70`	`auto result = log_move(*it_);`
`60`		`- prev_move_ = it_;`
`61`	`71`	`++it_;`
`62`		`- return result;`
	`72`	`+ return result ? MBFCircleClientState::MOVING : MBFCircleClientState::FAILED;`
`63`	`73`	`}`
`64`	`74`
`65`	`75`	`bool at_end()`
`66`	`76`	`{`
`67`	`77`	`return it_ == pose_goals_.end();`
`68`	`78`	`}`
`69`	`79`
`70`		`- bool prev_move()`
`71`		`- {`
	`80`	`+ MBFCircleClientState prev_move()`
	`81`	`+ {`
	`82`	`+ if (terminate_)`
	`83`	`+ {`
	`84`	`+ return MBFCircleClientState::AT_END;`
	`85`	`+ }`
	`86`	`+`
`72`	`87`	`if (prev_move_ != pose_goals_.begin())`
`73`	`88`	`{`
`74`	`89`	`auto result = log_move(*prev_move_);`
`75`	`90`	`it_ = prev_move_+1;`
`76`		`- prev_move_ = it_-1;`
`77`		`- return result;`
	`91`	`+ --prev_move_;`
	`92`	`+ return result ? MBFCircleClientState::MOVING : MBFCircleClientState::FAILED;`
`78`	`93`	`}`
`79`	`94`	`else`
`80`	`95`	`{`
`81`		`- return driveHome();`
	`96`	`+ return driveHome() ? MBFCircleClientState::MOVING : MBFCircleClientState::FAILED;`
`82`	`97`	`}`
`83`	`98`	`}`
`84`	`99`
`@@ -93,6 +108,7 @@ struct MBFClient`
`93`	`108`	`std::vector<mbf_msgs::MoveBaseGoal>::const_iterator prev_move_;`
`94`	`109`	`const mbf_msgs::MoveBaseGoal& home_;`
`95`	`110`	`actionlib::SimpleActionClient<mbf_msgs::MoveBaseAction> ac_;`
	`111`	`+ bool terminate_;`
`96`	`112`	`};`
`97`	`113`
`98`	`114`	`} // end`