Merge branch 'master' into fetch-go-to-kitchen

jsk_fetch_robot/fetcheus/fetch-interface.l

@@ -95,6 +95,10 @@
    (if (member :clear-velocities args) (setq clear-velocities (cadr (member :clear-velocities args))))
    ;; for simulation mode
    (when (send self :simulation-modep)
+     (if (member :use-torso args)
+       (setq args (append (subseq args 0 (position :use-torso args))
+                          (if (> (length args) (+ 2 (position :use-torso args)))
+                            (subseq args (+ (position :use-torso args) 2))))))
      (return-from :angle-vector (send* self :angle-vector-raw av tm ctype start-time args)))
    ;;
    (when (not (numberp tm))
@@ -125,6 +129,10 @@
    (if (member :clear-velocities args) (setq clear-velocities (cadr (member :clear-velocities args))))
    ;; for simulation mode
    (when (send self :simulation-modep)
+     (if (member :use-torso args)
+       (setq args (append (subseq args 0 (position :use-torso args))
+                          (if (> (length args) (+ 2 (position :use-torso args)))
+                            (subseq args (+ (position :use-torso args) 2))))))
      (return-from :angle-vector-sequence
                   (send* self :angle-vector-sequence-raw avs tms ctype start-time args)))
    (unless (and (listp tms) (every #'numberp tms))
@@ -364,7 +372,11 @@ Example: (send self :gripper :position) => 0.00"
 (defun fetch-init (&optional (create-viewer))
   (unless (boundp '*fetch*) (fetch) (send *fetch* :reset-pose))
   (unless (ros::ok) (ros::roseus "fetch_eus_interface"))
-  (unless (boundp '*co*) (setq *co* (instance collision-object-publisher :init)))
+  (unless (boundp '*co*)
+    (ros::ros-warn ";; collision-object-publisher wait for \"apply_planning_scene\" service for ~A sec~%" 5)
+    (if (ros::wait-for-service "apply_planning_scene" 5)
+      (setq *co* (instance collision-object-publisher :init))
+      (ros::ros-warn ";; could not find \"apply_planning_scene\" skip creating *co*~%")))
   (unless (boundp '*ri*) (setq *ri* (instance fetch-interface :init)))
 
   (ros::spin-once)

jsk_fetch_robot/fetcheus/test/test-fetcheus.l

@@ -126,6 +126,20 @@
             (format nil ":go-velocity returns t with argument ~A" go-vel-arg))
     ))
 
+(deftest fetch-use-torso
+  (let ()
+    (setq *ri* (instance fetch-interface :init))
+    (assert (send *ri* :angle-vector (send *fetch* :angle-vector) 3000 :use-torso t)
+            "failed to do :angle-vector with :use-torso in kinematics simulator")
+    (assert (send *ri* :angle-vector-sequence
+                  (list (send *fetch* :init-pose) (send *fetch* :angle-vector))
+                  (list 3000 3000) :use-torso t)
+            "failed to do :angle-vector-sequence with :use-torso in kinematics simulator")
+    ))
+
+(deftest fetch-init-test
+  (let ()
+    (fetch-init)))
 
 (run-all-tests)
 (exit)

jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch

@@ -126,7 +126,7 @@
   <group if="$(arg launch_move_base)">
     <!-- jsk_maps -->
     <include file="$(find jsk_maps)/launch/start_map_$(arg map_frame).launch">
-      <arg name="launch_map_server" value="false" />
+      <arg name="launch_map_server" value="true" />
     </include>
 
     <!-- dock localization -->
@@ -137,7 +137,8 @@
       <arg name="map_file" value="$(arg map_file)" />
       <arg name="map_keepout_file" value="$(arg keepout_map_file)" />
       <arg name="use_keepout" value="true" />
-
+      <arg name="use_map_topic" value="true" />
+      <arg name="launch_map_server" value="false" />
       <arg name="odom_topic" value="/odom_combined" />
     </include>
 

jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/move_base.xml

@@ -1,6 +1,10 @@
 <!-- original : pr2_navigation_global/move_base.xml (electric) -->
 <launch>
   <arg name="use_teb_local_planner" default="false" />
+  <arg name="use_map_keepout" default="false" />
+  <arg name="map_topic" value="map_keepout" if="$(arg use_map_keepout)" />
+  <arg name="map_topic" value="map" unless="$(arg use_map_keepout)" />
+
   <include file="$(find pr2_machine)/$(env ROBOT).machine" />
 
   <!-- Bring up the tilt_laser aware version of move_base -->
@@ -12,6 +16,7 @@
     <!-- launch-prefix="nice -n +10"> -->
     <remap from="odom" to="base_odometry/odom" />
     <remap from="cmd_vel" to="navigation/cmd_vel/raw" />
+    <remap from="map" to="$(arg map_topic)" />
     <remap from="~clear_costmaps" to="/move_base/clear_costmaps" />
 
     <param name="base_global_planner" value="navfn/NavfnROS" />

jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/pr2_2dnav.launch

@@ -4,6 +4,7 @@
   <arg name="launch_look_forward" default="true"/>
   <arg name="launch_teleop_joystick" default="true" />
   <arg name="launch_navigation" default="true" />
+  <arg name="use_map_keepout" default="false" />
 
   <include file="$(find pr2_machine)/$(env ROBOT).machine" />
 
@@ -17,7 +18,9 @@
     <include file="$(find jsk_pr2_startup)/jsk_pr2_sensors/lasers_and_filters.xml">
       <arg name="gazebo" value="$(arg gazebo)"/>
     </include>
-    <include file="$(find jsk_pr2_startup)/jsk_pr2_move_base/move_base.xml" />
+    <include file="$(find jsk_pr2_startup)/jsk_pr2_move_base/move_base.xml">
+      <arg name="use_map_keepout" value="$(arg use_map_keepout)" />
+    </include>
 
     <!-- To ignore tilt-laser -->
     <node name="tilt_laser_mux" pkg="topic_tools" type="mux"

jsk_pr2_robot/jsk_pr2_startup/pr2.launch

@@ -24,6 +24,8 @@
   <arg name="launch_reset_motors" default="true" />
   <arg name="slow_camera_framerate" default="true" />
   <arg name="launch_c3_startup" default="false" />
+  <arg name="use_map_keepout" default="true"
+       doc="whether to use keepout map or not for navigation (default: true)"/>
   <arg name="USER_NAME" default="false" />
 
   <include file="$(find jsk_pr2_startup)/jsk_pr2.machine"
@@ -36,6 +38,8 @@
   <include if="$(arg launch_move_base)"
            file="$(find jsk_pr2_startup)/jsk_pr2_move_base/pr2_2dnav.launch">
     <arg name="launch_look_forward" value="$(arg launch_look_forward)"/>
+    <arg name="launch_navigation" value="true" />
+    <arg name="use_map_keepout" value="$(arg use_map_keepout)" />
   </include>
 
   <!-- special start: use when launuch_move_base is false-->
@@ -52,6 +56,7 @@
   <include file="$(find jsk_maps)/launch/start_map_$(arg map_frame).launch"
            if="$(arg launch_map)">
     <arg name="MACHINE" value="c2" />
+    <arg name="keepout" value="$(arg use_map_keepout)" />
   </include>
 
   <!-- japanese speech node -->

jsk_pr2_robot/jsk_pr2_startup/pr2_bringup.launch

@@ -4,7 +4,7 @@
   <arg name="launch_hd_monitor" default="true" />
   <arg name="launch_ntp_monitor" default="true"/>
   <arg name="launch_wifi_ddwrt" default="false" />
-  <arg name="launch_network_detector" default="false" />
+  <arg name="launch_network_detector" default="true" />
   <arg name="launch_app_manager" default="true" />
   <arg name="setup_mannequin_mode" default="true"
        doc="prepares mannequin_mode to be callable by EusLisp interface"/>
@@ -169,7 +169,13 @@
       <param name="router" type="string" value="wifi-router" />
     </node>
   </group>
-
+
+  <!-- Network detector -->
+  <node pkg="pr2_computer_monitor" type="network_detector" name="network_detector"
+        if="$(arg launch_network_detector)">
+    <param name="interface_name" value="wan0"/>
+  </node>
+
   <!-- Diagnostics aggregation -->
   <node pkg="diagnostic_aggregator" type="aggregator_node"  name="diag_agg">
     <rosparam command="load" file="$(find jsk_pr2_startup)/jsk_pr2_warning/jsk_pr2_analyzers.yaml"/>
@@ -198,11 +204,6 @@
     <param name="buffer_size" value="120.0"/>
   </node>
 
-  <node pkg="pr2_computer_monitor" type="network_detector" name="network_detector"
-        if="$(arg launch_network_detector)">
-    <param name="interface_name" value="wan0"/>
-  </node>
-
   <!-- logging database -->
   <!-- NOTE: parameter robot/type and robot/name are defined at /etc/ros/robot.yaml -->
   <!-- expected mongod server runs on c2 -->