add mbf prev/next client

Author: juliangaal

advanced/CMakeLists.txt

@@ -4,37 +4,23 @@ project(mbf_advanced)
 set(CMAKE_CXX_STANDARD 14)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
 
-find_package(BehaviorTreeV3)
+find_package(BehaviorTreeV3 REQUIRED)
+
 find_package(catkin REQUIRED COMPONENTS
   roscpp
   geometry_msgs
   actionlib
 )
 
-###################################
-## catkin specific configuration ##
-###################################
-
 catkin_package(
-#  DEPENDS move_base_flex
+  DEPENDS move_base_flex
 )
 
 catkin_install_python(
-  PROGRAMS scripts/circle_smach.py
+  PROGRAMS scripts/circle_smach.py scripts/pytrees.py scripts/mbf_client.py
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 
-add_executable(behavior_tree src/behavior_tree.cpp)
-target_include_directories(behavior_tree PRIVATE include ${catkin_INCLUDE_DIRS})
-target_link_libraries(behavior_tree BT::behaviortree_cpp_v3 ${catkin_LIBRARIES})
-target_compile_definitions(behavior_tree
-        PRIVATE
-        BT_XML_PATH="${PROJECT_SOURCE_DIR}/param/behavior_tree.xml"
-        POSE_PATH="${PROJECT_SOURCE_DIR}/param/poses.txt"
-)
-
-add_dependencies(behavior_tree ${behavior_tree_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
 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})

advanced/include/mbf_advanced/mbf_cpp_client.h

@@ -73,6 +73,7 @@ struct MBFClient
         {
             auto result = log_move(*prev_move_);
             it_ = prev_move_+1;
+            prev_move_ = it_-1;
             return result;
         }
         else

advanced/package.xml

@@ -2,7 +2,7 @@
 <package format="2">
   <name>mbf_advanced</name>
   <version>0.0.0</version>
-  <description>Minimal example for move base flex and turtlebot</description>
+  <description>Minimal examples for move base flex and turtlebot</description>
 
   <maintainer email="[email protected]">Julian Gaal</maintainer>
 
@@ -17,4 +17,6 @@
   <exec_depend>clear_costmap_recovery</exec_depend>
   <exec_depend>rotate_recovery</exec_depend>
   <exec_depend>behaviortree_cpp_v3</exec_depend>
+  <exec_depend>py_trees_ros</exec_depend>
+  <exec_depend>moveback_recovery</exec_depend>
 </package>

advanced/scripts/mbf_client.py

@@ -0,0 +1,84 @@
+import rospy
+import mbf_msgs.msg as mbf_msgs
+
+def create_goal(x, y, z, xx, yy, zz, ww):
+    goal = mbf_msgs.MoveBaseGoal()
+    goal.target_pose.header.frame_id = "map"
+    goal.target_pose.header.stamp = rospy.Time.now()
+    goal.target_pose.pose.position.x = x
+    goal.target_pose.pose.position.y = y
+    goal.target_pose.pose.position.z = z
+    goal.target_pose.pose.orientation.x = xx
+    goal.target_pose.pose.orientation.y = yy
+    goal.target_pose.pose.orientation.z = zz
+    goal.target_pose.pose.orientation.w = ww
+    return goal
+
+
+def goal_generator():
+    target_poses = [   
+        create_goal(-1.75, 0.74, 0, 0, 0, 0.539, 0.843),
+        create_goal(-0.36, 1.92, 0, 0, 0, -0.020, 0.999),
+        create_goal(0.957, 1.60, 0, 0, 0, -0.163, 0.987),
+        create_goal(1.8741, 0.3830, 0, 0, 0, -0.70, 0.711),
+        create_goal(1.752, -0.928, 0, 0, 0, -0.856, 0.517),
+        create_goal(0.418, -2.116, 0, 0, 0, 0.998, 0.0619),
+        create_goal(-0.775, -1.80, 0, 0, 0, 0.954, 0.300),
+        create_goal(-1.990, -0.508, 0, 0, 0, -0.112, 0.999)
+    ]
+
+    for target_pose in target_poses:
+        yield target_pose
+
+
+class Goals:
+    def __init__(self):
+        self.goals = [   
+            create_goal(-1.75, 0.74, 0, 0, 0, 0.539, 0.843),
+            create_goal(-0.36, 1.92, 0, 0, 0, -0.020, 0.999),
+            create_goal(0.957, 1.60, 0, 0, 0, -0.163, 0.987),
+            create_goal(1.8741, 0.3830, 0, 0, 0, -0.70, 0.711),
+            create_goal(1.752, -0.928, 0, 0, 0, -0.856, 0.517),
+            create_goal(0.418, -2.116, 0, 0, 0, 0.998, 0.0619),
+            create_goal(-0.775, -1.80, 0, 0, 0, 0.954, 0.300),
+            create_goal(-1.990, -0.508, 0, 0, 0, -0.112, 0.999)
+        ]
+
+        self.iterator = 0
+        self.next_called = False
+
+    def next(self):
+        goal = self.goals[self.iterator]
+        self.iterator += 1
+        self.next_called = True
+
+        if self.iterator > len(self.goals)-1:
+            raise IndexError
+
+        return goal
+
+    def prev(self):
+        if self.next_called:
+            self.iterator -= 2
+            self.next_called = False
+        else:
+            self.iterator -= 1
+        
+        if self.iterator < 0:
+            raise IndexError
+
+        return self.goals[self.iterator]
+
+class MBFClient:
+    def __init__(self):
+        self.goals = Goals()
+
+    def next(self):
+        return self.goals.next()
+       
+    def prev(self):
+        return self.goals.prev()
+
+if __name__ == '__main__':
+    rospy.init_node("move_base_flex_client")
+    mbf = MBFClient()