add PR2 Fridge demo + Behavior Tree

detect_cans_in_fridge_201202/euslisp/main.l

@@ -37,14 +37,18 @@
 
 
 (defun restore-params ()
-  (if (boundp '*global-inflation-radius*)
-    (ros::set-dynamic-reconfigure-param
-      "/move_base_node/global_costmap/inflation_layer" "inflation_radius"
-      :double *global-inflation-radius*))
-  (if (boundp '*local-inflation-radius*)
-    (ros::set-dynamic-reconfigure-param
-      "/move_base_node/local_costmap/inflation_layer" "inflation_radius"
-      :double *local-inflation-radius*))
+  (let ((global-costmap-server "/move_base_node/global_costmap/inflation_layer")
+        (local-costmap-server "/move_base_node/local_costmap/inflation_layer"))
+    (if (and (boundp '*global-inflation-radius*)
+             (ros::wait-for-service (format nil "~A/set_parameters" global-costmap-server) 1))
+      (ros::set-dynamic-reconfigure-param
+        global-costmap-server "inflation_radius"
+        :double *global-inflation-radius*))
+    (if (and (boundp '*local-inflation-radius*)
+             (ros::wait-for-service (format nil "~A/set_parameters" local-costmap-server) 1))
+      (ros::set-dynamic-reconfigure-param
+        local-costmap-server "inflation_radius"
+        :double *local-inflation-radius*)))
   t)
 
 

fridge_demo_bt/CMakeLists.txt

@@ -0,0 +1,56 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(fridge_demo_bt)
+
+find_package(catkin REQUIRED COMPONENTS
+  message_generation
+  roscpp
+  behaviortree_ros
+  roseus_bt
+  std_msgs
+  actionlib_msgs
+)
+find_package(fmt)
+
+add_service_files(
+  FILES
+
+)
+
+add_action_files(
+  FILES
+  CloseFridge.action
+  Finish.action
+  FinishFailure.action
+  GraspCan.action
+  HandOverCan.action
+  Init.action
+  MoveToFridge.action
+  MoveToInitialPosition.action
+  OpenFridge.action
+)
+
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+  actionlib_msgs
+)
+
+catkin_package(
+  INCLUDE_DIRS
+  LIBRARIES
+  CATKIN_DEPENDS
+  message_runtime
+  std_msgs
+  actionlib_msgs
+  DEPENDS fmt
+)
+
+
+include_directories(${catkin_INCLUDE_DIRS})
+add_subdirectory(${roseus_bt_SOURCE_PREFIX}/include/rosbridgecpp rosbridgecpp)
+
+add_executable(fridge_demo src/fridge_demo.cpp)
+add_dependencies(fridge_demo ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(fridge_demo ${catkin_LIBRARIES} rosbridgecpp fmt::fmt)
+
+

fridge_demo_bt/action/CloseFridge.action

@@ -0,0 +1,6 @@
+
+---
+bool success
+---
+string update_field_name
+

fridge_demo_bt/action/Finish.action

@@ -0,0 +1,6 @@
+
+---
+bool success
+---
+string update_field_name
+

fridge_demo_bt/action/FinishFailure.action

@@ -0,0 +1,6 @@
+
+---
+bool success
+---
+string update_field_name
+

fridge_demo_bt/action/GraspCan.action

@@ -0,0 +1,6 @@
+string can_name
+---
+bool success
+---
+string update_field_name
+

fridge_demo_bt/action/HandOverCan.action

@@ -0,0 +1,6 @@
+
+---
+bool success
+---
+string update_field_name
+

fridge_demo_bt/action/Init.action

@@ -0,0 +1,6 @@
+string can_name
+---
+bool success
+---
+string update_field_name
+string can_name

fridge_demo_bt/action/MoveToFridge.action

@@ -0,0 +1,6 @@
+
+---
+bool success
+---
+string update_field_name
+

fridge_demo_bt/action/MoveToInitialPosition.action

@@ -0,0 +1,6 @@
+
+---
+bool success
+---
+string update_field_name
+

fridge_demo_bt/action/OpenFridge.action

@@ -0,0 +1,6 @@
+
+---
+bool success
+---
+string update_field_name
+

fridge_demo_bt/euslisp/fridge-demo-action-server.l

@@ -0,0 +1,92 @@
+(ros::roseus "action_server")
+(ros::load-ros-package "fridge_demo_bt")
+(load "package://roseus_bt/euslisp/nodes.l")
+
+(require :main "package://detect_cans_in_fridge_201202/euslisp/main.l")
+
+;; TODO: parameters should be arguments
+(defvar *use-arm* :larm)
+(defvar *use-base* nil)
+
+
+;; define callbacks
+(defun close-fridge-execute-cb (server goal)
+  (close-fridge :use-arm *use-arm*))
+
+(defun finish-execute-cb (server goal)
+  (restore-params)
+  t)
+
+(defun finish-failure-execute-cb (server goal)
+  (restore-params)
+  t)
+
+(defun grasp-can-execute-cb (server goal)
+  (let ((can_name (send goal :goal :can_name)))
+    (grasp-can :use-arm *use-arm*)))
+
+(defun hand-over-can-execute-cb (server goal)
+  (pass-to-human :use-arm *use-arm*))
+
+(defun init-execute-cb (server goal)
+  (let ((can_name (send goal :goal :can_name)))
+    (ros::ros-warn "can name is ~A" can_name)
+    (initialize-demo can_name)
+    (store-params)
+    (inflation-loose :local-costmap-inflation-radius 0.1
+                     :global-costmap-inflation-radius 0.1)
+    t))
+
+(defun move-to-fridge-execute-cb (server goal)
+  (goto-front-of-fridge))
+
+(defun move-to-initial-position-execute-cb (server goal)
+  (goto-initial-position :use-arm *use-arm*))
+
+(defun open-fridge-execute-cb (server goal)
+  (open-fridge-door :use-arm *use-arm* :use-base *use-base*))
+
+
+;; create server instances
+(instance roseus_bt:action-node :init
+          "fridge_demo/close_fridge" fridge_demo_bt::CloseFridgeAction
+          :execute-cb 'close-fridge-execute-cb)
+
+(instance roseus_bt:action-node :init
+          "fridge_demo/finish" fridge_demo_bt::FinishAction
+          :execute-cb 'finish-execute-cb)
+
+(instance roseus_bt:action-node :init
+          "fridge_demo/finish_failure" fridge_demo_bt::FinishFailureAction
+          :execute-cb 'finish-failure-execute-cb)
+
+(instance roseus_bt:action-node :init
+          "fridge_demo/grasp_can" fridge_demo_bt::GraspCanAction
+          :execute-cb 'grasp-can-execute-cb)
+
+(instance roseus_bt:action-node :init
+          "fridge_demo/hand_over_can" fridge_demo_bt::HandOverCanAction
+          :execute-cb 'hand-over-can-execute-cb)
+
+(instance roseus_bt:action-node :init
+          "fridge_demo/init" fridge_demo_bt::InitAction
+          :execute-cb 'init-execute-cb)
+
+(instance roseus_bt:action-node :init
+          "fridge_demo/move_to_fridge" fridge_demo_bt::MoveToFridgeAction
+          :execute-cb 'move-to-fridge-execute-cb)
+
+(instance roseus_bt:action-node :init
+          "fridge_demo/move_to_initial_position" fridge_demo_bt::MoveToInitialPositionAction
+          :execute-cb 'move-to-initial-position-execute-cb)
+
+(instance roseus_bt:action-node :init
+          "fridge_demo/open_frirdge" fridge_demo_bt::OpenFridgeAction
+          :execute-cb 'open-fridge-execute-cb)
+
+
+;; set rate
+(ros::rate 100)
+
+;; spin
+(roseus_bt:spin)

fridge_demo_bt/fc.rosinstall

@@ -0,0 +1,44 @@
+- git:
+    local-name: behaviortree_cpp
+    uri: https://github.com/Affonso-Gui/BehaviorTree.CPP.git
+    version: state_aware_bt
+- git:
+    local-name: behaviortree_ros
+    uri: https://github.com/BehaviorTree/BehaviorTree.ROS.git
+    version: master
+- git:
+    local-name: euslime
+    uri: https://github.com/Affonso-Gui/euslime.git
+    version: eus10
+- git:
+    local-name: euslisp/Euslisp
+    uri: https://github.com/Affonso-Gui/EusLisp.git
+    version: eus10
+- git:
+    local-name: euslisp/jskeus
+    uri: https://github.com/euslisp/jskeus.git
+    version: master
+- git:
+    local-name: groot
+    uri: https://github.com/Affonso-Gui/Groot.git
+    version: roseus_bt_state_aware
+- git:
+    local-name: jsk-ros-pkg/geneus
+    uri: https://github.com/jsk-ros-pkg/geneus.git
+    version: master
+- git:
+    local-name: jsk-ros-pkg/jsk_demos
+    uri: https://github.com/jsk-ros-pkg/jsk_demos.git
+    version: fridge-demo-bt
+- git:
+    local-name: jsk-ros-pkg/jsk_pr2eus
+    uri: https://github.com/jsk-ros-pkg/jsk_pr2eus.git
+    version: master
+- git:
+    local-name: jsk-ros-pkg/jsk_roseus
+    uri: https://github.com/Affonso-Gui/jsk_roseus.git
+    version: eus10_state_aware
+- git:
+    local-name: roseus_resume
+    uri: https://github.com/Affonso-Gui/roseus_resume.git
+    version: eus10

fridge_demo_bt/launch/fridge_demo_server.launch

@@ -0,0 +1,20 @@
+<launch>
+  <arg name="main" default="true" />
+  <arg name="can_name" default="georgia" />
+
+  <node if="$(arg main)" pkg="fridge_demo_bt" type="fridge_demo" name="fridge_demo_main"
+        output="screen">
+    <rosparam subst_value="true">
+      can_name: $(arg can_name)
+    </rosparam>
+  </node>
+
+  <include file="$(find rosbridge_server)/launch/rosbridge_websocket.launch">
+    <arg name="unregister_timeout" value="100000"/>
+  </include>
+
+  <node pkg="roseus" type="roseus" name="fridge_demo_action_server"
+        args="$(find fridge_demo_bt)/euslisp/fridge-demo-action-server.l"
+        output="screen"/>
+
+</launch>

fridge_demo_bt/models/fridge_demo.xml

@@ -0,0 +1,42 @@
+<?xml version="1.0"?>
+<root main_tree_to_execute="BehaviorTree">
+    <!-- ////////// -->
+    <BehaviorTree ID="BehaviorTree">
+        <SequenceStar>
+            <Action ID="Init" can_name="${can_name}"/>
+            <Fallback>
+                <Sequence>
+                    <Action ID="MoveToFridge"/>
+                    <Action ID="OpenFridge"/>
+                    <Action ID="GraspCan" can_name="${can_name}"/>
+                    <Action ID="CloseFridge"/>
+                    <ForceSuccess>
+                        <Action ID="MoveToInitialPosition"/>
+                    </ForceSuccess>
+                    <Action ID="HandOverCan"/>
+                    <Action ID="Finish"/>
+                </Sequence>
+                <ForceFailure>
+                    <Action ID="FinishFailure"/>
+                </ForceFailure>
+            </Fallback>
+        </SequenceStar>
+    </BehaviorTree>
+    <!-- ////////// -->
+    <TreeNodesModel>
+        <Action ID="CloseFridge" server_name="fridge_demo/close_fridge"/>
+        <Action ID="Finish" server_name="fridge_demo/finish"/>
+        <Action ID="FinishFailure" server_name="fridge_demo/finish_failure"/>
+        <Action ID="GraspCan" server_name="fridge_demo/grasp_can">
+            <input_port default="georgia" name="can_name" type="string">name of can in fridge</input_port>
+        </Action>
+        <Action ID="HandOverCan" server_name="fridge_demo/hand_over_can"/>
+        <Action ID="Init" server_name="fridge_demo/init">
+            <inout_port default="can_name" name="can_name" type="string">name of can in fridge</inout_port>
+        </Action>
+        <Action ID="MoveToFridge" server_name="fridge_demo/move_to_fridge"/>
+        <Action ID="MoveToInitialPosition" server_name="fridge_demo/move_to_initial_position"/>
+        <Action ID="OpenFridge" server_name="fridge_demo/open_frirdge"/>
+    </TreeNodesModel>
+    <!-- ////////// -->
+</root>

fridge_demo_bt/package.xml

@@ -0,0 +1,46 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>fridge_demo_bt</name>
+  <version>0.0.0</version>
+  <description>The fridge_demo_bt package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <maintainer email="[email protected]">The Author</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/fridge_demo_bt</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="[email protected]">The Author</author> -->
+
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>behaviortree_ros</build_depend>
+  <build_depend>roseus_bt</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>actionlib_msgs</build_depend>
+
+  <exec_depend>message_runtime</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>behaviortree_ros</exec_depend>
+  <exec_depend>roseus_bt</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>actionlib_msgs</exec_depend>
+
+  <export>
+  </export>
+</package>