Merge pull request #643 from snozawa/update_footstep_planner_sample

Update footstep planner sample

Author: snozawa

jsk_footstep_controller/euslisp/util.l

@@ -374,3 +374,71 @@ You can create `*ri*' like
     ret-coords
     )
   )
+
+;; Functions to use footstep planning from euslisp
+(defun initialize-eus-footstep-planning-client
+  ()
+  "Initialize footstep planning client from euslisp."
+  (ros::roseus "footstep_planner_client")
+  ;; For obstacles
+  (ros::advertise "/footstep_planner/obstacle_model" sensor_msgs::PointCloud2 1)
+  (ros::load-ros-manifest "sensor_msgs")
+  ;; For visualization
+  (ros::advertise "/footstep_from_marker" jsk_footstep_msgs::FootstepArray 1)
+  ;; For footstep planning action server
+  (setq *footstep-planning-client* (instance ros::simple-action-client :init
+                                             "footstep_planner" jsk_footstep_msgs::PlanFootstepsAction))
+  (unix:sleep 3)
+  (ros::ros-info "waiting actionlib server")
+  (send *footstep-planning-client* :wait-for-server)
+  (ros::ros-info "waited actionlib server")
+  )
+
+(defun plan-footstep-from-goal-coords
+  (goal-coords &key (publish-result t) (start-coords (make-coords)) (frame-id "odom"))
+  "Plan footstep from goal-coords."
+  (let ((goal (make-footstep-planning-msgs
+               start-coords goal-coords :frame-id frame-id)))
+    (ros::ros-info "sending goal")
+    (send *footstep-planning-client* :send-goal goal)
+    (ros::ros-info "waiting for result")
+    (send *footstep-planning-client* :wait-for-result)
+    ;; (ros::ros-info "result: ~A" (send *footstep-planning-client* :get-result))
+    (when publish-result
+      (ros::publish "/footstep_from_marker" (send (send *footstep-planning-client* :get-result) :result)))
+    (send (send *footstep-planning-client* :get-result) :result)
+    ))
+
+(defun publish-footstep-planning-obstacle-model-from-eus-pointcloud
+  (a-pointcloud &key (frame-id "odom"))
+  "Publish obsacle_model used in footstep planning.
+   A eus-style pointcloud is required as an argument."
+  (ros::publish "/footstep_planner/obstacle_model" (make-ros-msg-from-eus-pointcloud a-pointcloud :frame frame-id))
+  )
+
+(defun get-pointcloud-within-bodies-2D
+  (blist &key (return-point-cloud t) ((:resolution dif) 50.0))
+  "Get pointcloud within bodies.
+   blist is list of bodies.
+   All bodies are assumed to be vertical prisms, so inclusion check is 2D."
+  (labels
+      ((get-points-within-object-2D-tmp
+        (bb)
+        (let* ((ret1) (vv (send bb :vertices))
+               (xmin (elt (find-extream vv #'(lambda (x) (elt x 0)) #'<) 0))
+               (xmax (elt (find-extream vv #'(lambda (x) (elt x 0)) #'>) 0))
+               (ymin (elt (find-extream vv #'(lambda (x) (elt x 1)) #'<) 1))
+               (ymax (elt (find-extream vv #'(lambda (x) (elt x 1)) #'>) 1))
+               (zmin (elt (find-extream vv #'(lambda (x) (elt x 0)) #'<) 2))
+               (zmax (elt (find-extream vv #'(lambda (x) (elt x 0)) #'>) 2)))
+          (do ((x (- xmin dif) (+ dif x))) ((< (+ dif xmax) x))
+              (do ((y (- ymin dif) (+ dif y))) ((< (+ ymax dif) y))
+                  (let ((z (* 0.5 (+ zmin zmax))))
+                    (when (not (eq :outside (send bb :insidep (float-vector x y z))))
+                      (push (float-vector x y z) ret1))
+                    )))
+          ret1)))
+    (let ((ret1 (apply #'append (mapcar #'get-points-within-object-2D-tmp blist))))
+      (if return-point-cloud
+          (instance pointcloud :init :points ret1)
+        ret1))))

jsk_footstep_planner/euslisp/footstep-planner-client-sample.l

@@ -3,46 +3,21 @@
 (ros::load-ros-manifest "jsk_footstep_planner")
 (load "package://jsk_footstep_controller/euslisp/util.l")
 
-(defun init ()
-  (ros::roseus "footstep_planner_client")
-  (ros::advertise "/footstep_from_marker" jsk_footstep_msgs::FootstepArray 1)
-  (setq *client* (instance ros::simple-action-client :init
-                           "footstep_planner" jsk_footstep_msgs::PlanFootstepsAction))
-  (unix:sleep 3)
-  (ros::ros-info "waiting actionlib server")
-  (send *client* :wait-for-server)
-  (ros::ros-info "waited actionlib server")
-  )
-
-(defun send-goal (goal-coords &key (publish-result t) (start-coords (make-coords)))
-  (let ((goal (make-footstep-planning-msgs
-               start-coords goal-coords :frame-id "odom")))
-    (ros::ros-info "sending goal")
-    (send *client* :send-goal goal)
-    (ros::ros-info "waiting for result")
-    (send *client* :wait-for-result)
-    ;; (ros::ros-info "result: ~A" (send *client* :get-result))
-    (when publish-result
-      (ros::publish "/footstep_from_marker" (send (send *client* :get-result) :result)))
-    (send (send *client* :get-result) :result)
-    ))
-
 (warn "
 ## launch simple node (not using pointcloud)
 roslaunch jsk_footstep_planner optimistic_footstep_planner.launch USE_CONTROLLER:=false USE_MARKER:=false USE_PERCEPTION:=false
 
 ")
-
-(init)
-(setq result (send-goal (make-coords :pos (float-vector 3000 0 0))))
+(initialize-eus-footstep-planning-client)
+(setq result (plan-footstep-from-goal-coords (make-coords :pos (float-vector 3000 0 0))))
 (let ((footstep-coords (footstep-array->coords result)))
   (print-readable-coords footstep-coords)
   ;; (send *ri* :set-foot-steps footstep-coords) ;; send real-robot if needed
   )
 (ros::spin-once)
 
 (warn "
-(setq result (send-goal (make-coords :pos (float-vector 3000 0 0))))
+(setq result (plan-footstep-from-goal-coords (make-coords :pos (float-vector 3000 0 0))))
 (let ((footstep-coords (footstep-array->coords result)))
   (print-readable-coords footstep-coords)
   ;; (send *ri* :set-foot-steps footstep-coords) ;; send real-robot if needed

jsk_footstep_planner/launch/cppplanner/footstep_planner.rviz

@@ -568,6 +568,36 @@ Visualization Manager:
       left: 0
       top: 750
       width: 128
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.01
+      Style: Flat Squares
+      Topic: /footstep_planner/obstacle_model
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48

jsk_footstep_planner/launch/cppplanner/optimistic_footstep_planner.launch

@@ -6,6 +6,7 @@
   <arg name="USE_PERCEPTION" default="true"/>
   <arg name="USE_SIMPLE_FOOTSTEP_CONTROLLER" default="true" />
   <arg name="GLOBAL_FRAME"   default="map" />
+  <arg name="USE_OBSTACLE_MODEL"   default="false" />
   <node pkg="dynamic_tf_publisher" type="tf_publish.py" name="dynamic_tf_publisher"
         output="log">
     <param name="use_cache" value="false"/>
@@ -17,18 +18,21 @@
     <!-- <remap from="footstep_planner/result" to="footstep_planner/result_non_refined" /> -->
     <rosparam subst_value="true">
       use_pointcloud_model: $(arg USE_PERCEPTION)
-      use_lazy_perception: true
+      use_lazy_perception: $(arg USE_PERCEPTION)
       use_local_movement: false
       use_transition_limit: true
       project_start_state: $(arg USE_PERCEPTION)
       project_goal_state: $(arg USE_PERCEPTION)
+      use_obstacle_model: $(arg USE_OBSTACLE_MODEL)
     </rosparam>
     <rosparam>
       resolution_x: 0.1
       resolution_y: 0.1
       resolution_theta: 0.1
       footstep_size_x: 0.24
       footstep_size_y: 0.14
+      collision_bbox_size: [0.4, 1.2, 1.0]
+      collision_bbox_offset: [0, 0, 0]
     </rosparam>
     <!-- successors -->
     <rosparam>

jsk_footstep_planner/src/footstep_graph.cpp

@@ -125,8 +125,9 @@ namespace jsk_footstep_planner
   bool FootstepGraph::isColliding(StatePtr current_state, StatePtr previous_state)
   {
     // if not use obstacle model, always return false
-    // to be collision-free always.
-    if (!use_obstacle_model_) {
+    // if use obstacle model and obstacle_model_ point cloud size is zero, always return false
+    // => to be collision-free always.
+    if (!use_obstacle_model_ || (obstacle_model_->size() == 0) ) {
       return false;
     }
     // compute robot coorde

jsk_footstep_planner/test/test_footstep_planning_eus_client.l

@@ -0,0 +1,53 @@
+#!/usr/bin/env roseus
+
+(require :unittest "lib/llib/unittest.l")
+(init-unit-test)
+
+(deftest test-sample-init
+  (assert
+   (progn
+     ;;(require "package://jsk_footstep_planner/euslisp/footstep-planner-client-sample.l")
+     (ros::load-ros-manifest "jsk_footstep_planner")
+     (load "package://jsk_footstep_controller/euslisp/util.l")
+     (initialize-eus-footstep-planning-client)
+     *footstep-planning-client*)))
+
+(deftest test-sample-without-obstacle
+  (assert
+   (progn
+     ;; Set empty point cloud == no obstacle
+     (publish-footstep-planning-obstacle-model-from-eus-pointcloud (instance pointcloud :init))
+     (let* ((result (plan-footstep-from-goal-coords (make-coords :pos (float-vector 3000 0 0)))))
+       (and result (footstep-array->coords result))
+       ))))
+
+(deftest test-sample-with-obstacle-1
+  (assert
+   (progn
+     (let ((obstacle-point-cloud
+            (get-pointcloud-within-bodies-2D (list (let ((bb (make-cube 400 400 50))) (send bb :translate (float-vector 400 0 0)) (send bb :worldcoords) bb)))))
+       ;; Set obstacle by cube
+       (publish-footstep-planning-obstacle-model-from-eus-pointcloud obstacle-point-cloud)
+       (let* ((result (plan-footstep-from-goal-coords (make-coords :pos (float-vector 3000 0 0)))))
+         (and result (footstep-array->coords result))
+         )))))
+
+(deftest test-sample-with-obstacle-2
+  (assert
+   (progn
+     (load "models/room73b2-scene")
+     (Objects (list (room73b2)))
+     (let ((obstacle-point-cloud
+            (get-pointcloud-within-bodies-2D
+             (list
+              (send (make-bounding-box (flatten (send-all (send (send *room73b2* :object "room73b2-gifuplastic-900-cart") :bodies) :vertices))) :body)
+              ))))
+       (publish-footstep-planning-obstacle-model-from-eus-pointcloud obstacle-point-cloud)
+       (let ((result (plan-footstep-from-goal-coords
+                      (send (send (send (send *room73b2* :object "room73b2-gifuplastic-900-cart") :copy-worldcoords) :translate (float-vector -600 -500 0) :world) :rotate pi/2 :z)
+                      :start-coords (send (send (send *room73b2* :object "room73b2-gifuplastic-900-cart") :copy-worldcoords) :translate (float-vector 0 300 0) :world))))
+         (and result (footstep-array->coords result))
+         )))))
+
+(run-all-tests)
+(exit 0)

jsk_footstep_planner/test/test_footstep_planning_eus_client.test

@@ -0,0 +1,13 @@
+<launch>
+  <env name="ROBOT" value="JAXON_RED"/>
+  <arg name="USE_RVIZ" default="false"/>
+  <include file="$(find jsk_footstep_planner)/launch/cppplanner/optimistic_footstep_planner.launch">
+    <arg name="USE_SIMPLE_FOOTSTEP_CONTROLLER" value="true"/>
+    <arg name="USE_PERCEPTION" value="false"/>
+    <arg name="USE_CONTROLLER" value="false"/>
+    <arg name="USE_MARKER" value="false"/>
+    <arg name="USE_OBSTACLE_MODEL" value="true"/>
+    <arg name="USE_RVIZ" value="$(arg USE_RVIZ)"/>
+  </include>
+  <test test-name="test_footstep_planning_eus_client" pkg="jsk_footstep_planner" type="test_footstep_planning_eus_client.l" time-limit="60.0"/>
+</launch>