[WIP] PR1040 Develop branch

jsk_fetch_robot/jsk_fetch_startup/launch/fetch_lifelog.xml

@@ -18,6 +18,7 @@
   </include>
 
   <include file="$(find jsk_robot_startup)/lifelog/common_logger.launch">
+    <arg name="approximate_sync" value="true"/>
     <arg name="save_rgb" value="true" />
     <arg name="save_depth" value="true" />
     <arg name="save_tf" value="true" />

jsk_fetch_robot/jsk_fetch_startup/package.xml

@@ -30,6 +30,7 @@
   <run_depend>move_base</run_depend>
 
   <!-- for fetch teleop -->
+  <run_depend>app_manager</run_depend>
   <run_depend>fetch_bringup</run_depend>
   <run_depend>safe_teleop_base</run_depend>
   <run_depend>joy</run_depend>
@@ -46,5 +47,6 @@
   <test_depend>rostest</test_depend>
 
   <export>
+    <app_manager app_dir="${prefix}/apps"/>
   </export>
 </package>

jsk_pr2_robot/jsk_pr2_startup/apps/tweet/tweet.l

@@ -17,6 +17,6 @@
 (speak-jp "カシャ！")
 
 (tweet-string "Hello!!"
-              :warning-time nil
+              :wait-for-camera 0
               :with-image "/kinect_head/rgb/image_rect_color")
 (exit)

jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/db_client.launch

@@ -23,6 +23,8 @@
     <arg name="launch_manager" value="false" />
     <arg name="manager" value="kinect_head/kinect_head_c2_nodelet_manager" />
     <arg name="map_frame_id" value="$(arg map_frame)" />
+    <arg name="debug" value="false"/>
+    <arg name="approximate_sync" value="true"/>
   </include>
 
   <rosparam ns="lifelog/joint_states_throttle">

jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/pr2-tweet-log.l

@@ -9,7 +9,7 @@
 (require :tweet-client "package://jsk_robot_startup/lifelog/tweet_client.l")
 (defparameter *mongo-database* "jsk_robot_lifelog")
 (defparameter *mongo-collection* "pr1012")
-(defparameter *json-parse-key-function* #'string->keyword)
+(defparameter *json-parse-key-function* #'json::string->keyword)
 (setq json::*tzoffset* -8)
 (defvar *tweet* t)
 (defparameter *query-limit* 100)
@@ -75,7 +75,7 @@
               (setq ret (send w :tweet))
               (when ret
                 (if *tweet*
-                    (tweet-string ret :warning-time nil :with-image with-image :speek nil :image-wait 10))
+                    (tweet-string ret :wait-for-camera 0 :with-image with-image :with-speech nil))
                 (warn "tweeted: ~A...~%" (subseq ret 0 100))
                 (setq with-image nil)
                 ))))

jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/pr2_microphone.launch

@@ -0,0 +1,44 @@
+<launch>
+  <arg name="use_microcone" default="false"/>
+  <arg name="use_respeaker" default="true"/>
+
+  <arg name="language" default="ja-JP"/>
+
+  <arg name="machine" default="c1" if="$(optenv USE_MIC_C1 false)"/>
+  <arg name="machine" default="c2" if="$(optenv USE_MIC_C2 false)"/>
+
+  <arg name="ROBOT" default="$(optenv ROBOT pr2)"/>
+  <include file="$(find pr2_machine)/$(arg ROBOT).machine"/>
+
+  <!-- microcone -->
+  <group if="$(arg use_microcone)">
+    <node name="pr2_hark" pkg="jsk_pr2_startup"
+          type="microcone_localization_MFCC.sh" output="log"
+          machine="$(arg machine)"/>
+  </group>
+
+  <!-- respeaker -->
+  <group if="$(arg use_respeaker)">
+    <node name="respeaker_node" pkg="respeaker_ros" type="respeaker_node.py"
+          machine="$(arg machine)" output="screen">
+      <rosparam>
+        sensor_frame_id: head_mount_link
+      </rosparam>
+    </node>
+
+    <node name="speech_to_text"
+          pkg="respeaker_ros" type="speech_to_text.py"
+          machine="$(arg machine)">
+      <remap from="audio" to="speech_audio"/>
+      <remap from="sound_play" to="robotsound_jp"/>
+      <rosparam subst_value="true">
+        language: $(arg language)
+        self_cancellation: true
+        tts_tolerance: 0.5
+        tts_action_names:
+            - robotsound
+            - robotsound_jp
+      </rosparam>
+    </node>
+  </group>
+</launch>

jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_lifelog/pr2_tweet.launch

@@ -11,7 +11,9 @@
   </include>
 
   <!-- tweet logging data -->
+  <!--
   <node name="pr2_tweet_log" pkg="roseus" type="roseus"
         args="$(find jsk_pr2_startup)/jsk_pr2_lifelog/pr2-tweet-log.l"
         machine="c2" output="screen" />
+  -->
 </launch>

jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/amcl_node.xml

@@ -2,7 +2,7 @@
 <launch>
 <node pkg="amcl" type="amcl" name="amcl" machine="c2"
       respawn="true" output="log">
-  <remap from="scan" to="base_scan" />
+  <remap from="scan" to="base_scan_filtered/scan" />
   <!-- Publish scans from best pose at a max of 10 Hz -->
   <param name="base_frame_id" value="/base_footprint"/>
   <param name="use_map_topic" value="true" />

jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/costmap_common_params.yaml

@@ -4,15 +4,15 @@ transform_tolerance: 1.0  # 0.2
 
 # parameters for static_layer
 static_layer:
-  map_topic: map
+  map_topic: /map
   subscribe_to_updates: true
 
 # parameters for obstacle_layer
 obstacle_layer:
-  observation_sources: base_scan_marking tilt_scan_filtered
-  base_scan_marking:
+  observation_sources: base_scan_filtered tilt_scan_filtered
+  base_scan_filtered:
     data_type: PointCloud2
-    topic: base_scan_marking
+    topic: /base_scan_filtered
     sensor_frame: base_laser_link
     marking: true
     clearing: true
@@ -24,13 +24,13 @@ obstacle_layer:
     raytrace_range: 3.0
   tilt_scan_filtered:
     data_type: PointCloud2
-    topic: tilt_scan_filtered
+    topic: /tilt_scan_filtered/navigation
     sensor_frame: laser_tilt_link
     marking: true
     clearing: true
-    observation_persistence: 1.0
+    observation_persistence: 2.5
     expected_update_rate: 0.25
-    min_obstacle_height: 0.0
+    min_obstacle_height: 0.05
     max_obstacle_height: 2.0
     obstacle_range: 2.5
     raytrace_range: 3.0

jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/global_costmap_params.yaml

@@ -1,6 +1,6 @@
 plugins:
   - {name: static_layer, type: "costmap_2d::StaticLayer" }
-  - {name: obstacle_layer, type: "costmap_2d::VoxelLayer" }
+  - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer" }
   - {name: inflation_layer,  type: "costmap_2d::InflationLayer" }
 
 footprint: [[-0.325, -0.325], [-0.425, -0.175], [-0.425, 0.175], [-0.325, 0.325], [0.325, 0.325], [0.375, 0.0], [0.325, -0.325]]

jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/local_costmap_params.yaml

@@ -1,12 +1,14 @@
 plugins:
-  - {name: obstacle_layer,  type: "costmap_2d::VoxelLayer" }
+  - {name: obstacle_layer,  type: "costmap_2d::ObstacleLayer" }
   - {name: inflation_layer,  type: "costmap_2d::InflationLayer" }
 
 # copied from pr2_navigation_global/config/local_costmap_params.yaml
 global_frame: odom_combined
 publish_frequency: 5.0
 static_map: false
 rolling_window: true
-width: 4.0 # 3.0
-height: 4.0 # 3.0
+width: 5.0 # 3.0
+height: 5.0 # 3.0
 resolution: 0.025 # 0.06
+
+footprint: [[-0.325, -0.325], [-0.425, -0.175], [-0.425, 0.175], [-0.325, 0.325], [0.325, 0.325], [0.375, 0.0], [0.325, -0.325]]

jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/move_base.xml

@@ -1,29 +1,28 @@
 <!-- original : pr2_navigation_global/move_base.xml (electric) -->
 <launch>
+  <arg name="use_teb_local_planner" default="false" />
   <include file="$(find pr2_machine)/$(env ROBOT).machine" />
 
   <!-- Bring up the tilt_laser aware version of move_base -->
   <node pkg="pr2_move_base" name="pr2_move_base_node" type="pr2_move_base.py"
         respawn="true" machine="c2" />
 
-  <!-- Throttle the voxel grid that is being published for rviz -->
-  <node ns="move_base_node/local_costmap"
-        name="voxel_grid_throttle" pkg="topic_tools" type="throttle"
-        machine="c2" args="messages voxel_grid 3.0 voxel_grid_throttled" />
-
   <node pkg="move_base" type="move_base" name="move_base_node"
-        machine="c2" clear_params="true" output="screen">
+        machine="c2" clear_params="true" output="screen" respawn="true">
     <!-- launch-prefix="nice -n +10"> -->
     <remap from="odom" to="base_odometry/odom" />
     <remap from="cmd_vel" to="navigation/cmd_vel/raw" />
     <remap from="~clear_costmaps" to="/move_base/clear_costmaps" />
 
     <param name="base_global_planner" value="navfn/NavfnROS" />
-    <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />
+    <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" unless="$(arg use_teb_local_planner)" />
+    <param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" if="$(arg use_teb_local_planner)" />
     <rosparam file="$(find jsk_pr2_startup)/jsk_pr2_move_base/navfn_ros_planner.yaml" command="load"
               ns="NavfnROS" />
     <rosparam file="$(find jsk_pr2_startup)/jsk_pr2_move_base/dwa_local_planner.yaml" command="load"
-              ns="DWAPlannerROS" />
+              ns="DWAPlannerROS" unless="$(arg use_teb_local_planner)" />
+    <rosparam file="$(find jsk_pr2_startup)/jsk_pr2_move_base/teb_local_planner_params.yaml" command="load"
+              if="$(arg use_teb_local_planner)" />
 
     <!-- Load common configuration files -->
     <rosparam file="$(find jsk_pr2_startup)/jsk_pr2_move_base/move_base_params.yaml" command="load" />

jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/pr2_2dnav.launch

@@ -17,13 +17,15 @@
     <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/ground_plane.xml" />
     <include file="$(find jsk_pr2_startup)/jsk_pr2_move_base/move_base.xml" />
 
     <!-- To ignore tilt-laser -->
     <node name="tilt_laser_mux" pkg="topic_tools" type="mux"
-          args="ground_object_cloud_mux ground_object_cloud empty_cloud">
+          args="tilt_scan_filtered/navigation tilt_scan_filtered empty_cloud">
       <remap from="mux" to="tilt_laser_mux"/>
+      <rosparam>
+        lazy: true
+      </rosparam>
     </node>
     <node name="empty_cloud_publisher" pkg="jsk_pr2_startup"
           type="publish_empty_cloud.py">

jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/safe_teleop.xml

@@ -31,13 +31,13 @@
     <param name="default_select" value="/input_vel"/>
   </node>
 
-  <node pkg="safe_teleop_base" type="safe_teleop_base" respawn="false"
-        name="safe_teleop_base" machine="c2">
+  <node pkg="safe_teleop_base" type="safe_teleop_base" respawn="true"
+        name="safe_teleop_base" machine="c2" clear_params="true">
+    <remap from="/odom" to="/base_odometry/odom"/>
     <remap from="base_velocity" to="/navigation/unsafe_vel"/>
     <remap from="~safe_vel" to="/navigation/safe_vel"/>
     <rosparam file="$(find jsk_pr2_startup)/jsk_pr2_move_base/safe_teleop_pr2_params.yaml"
               command="load" />
-    <rosparam param="local_costmap/footprint">[[-0.325, -0.325], [-0.325, 0.325], [0.325, 0.325], [0.46, 0.0], [0.325, -0.325]]</rosparam>
   </node>
 
   <!-- switch base velocity, safe or not -->

jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/safe_teleop_pr2_params.yaml

@@ -5,49 +5,55 @@ local_costmap:
   transform_tolerance: 0.2
   obstacle_range: 2.5
   raytrace_range: 3.0
-  inflation_layer:
-    inflation_radius: 0.2
-  inflation_radius: 0.2
+  footprint_clearing_enabled: true
   global_frame: odom_combined
   robot_base_frame: base_footprint
   update_frequency: 5.0
   publish_frequency: 2.0
   static_map: false
   rolling_window: true
 
-  width: 10.0
-  height: 10.0
+  width: 5.0
+  height: 5.0
   resolution: 0.025
   origin_x: 0.0
   origin_y: 0.0
 
-  publish_voxel_map: true
   map_type: voxel
   obstacle_layer:
     origin_z: 0.0
     z_voxels: 16
-    z_resolution: 0.081
+    z_resolution: 0.1
     unknown_cost_value: 0
-    unknown_threshold: 8
+    unknown_threshold: 4
     mark_threshold: 0
-  origin_z: 0.0
-  z_voxels: 16
-  z_resolution: 0.081
-  unknown_cost_value: 0
-  unknown_threshold: 8
-  mark_threshold: 0
-
-  observation_sources: base_scan_marking base_scan tilt_scan ground_object_cloud
-
-  base_scan_marking: {sensor_frame: base_laser_link, topic: /base_scan_marking, data_type: PointCloud2, expected_update_rate: 0.2,
-                      observation_persistence: 0.0, marking: true, clearing: false, min_obstacle_height: 0.08, max_obstacle_height: 2.0}
-
-  base_scan: {sensor_frame: base_laser_link, topic: /base_scan, data_type: LaserScan, expected_update_rate: 0.2,
-              observation_persistence: 0.0, marking: false, clearing: true, min_obstacle_height: -0.10, max_obstacle_height: 2.0}
-
-  tilt_scan: {sensor_frame: laser_tilt_link, topic: /tilt_scan, data_type: LaserScan, expected_update_rate: 0.2,
-              observation_persistence: 0.2, marking: false, clearing: true, min_obstacle_height: -20.00, max_obstacle_height: 40.0}
-
-  ground_object_cloud: {sensor_frame: laser_tilt_link, topic: /ground_object_cloud, data_type: PointCloud2, expected_update_rate: 0.2,
-                        observation_persistence: 4.6, marking: true, clearing: false, min_obstacle_height: -0.10, max_obstacle_height: 2.0}
+    publish_voxel_map: false
+  inflation_layer:
+    inflation_radius: 0.2
 
+  observation_sources: base_scan_filtered tilt_scan_filtered
+
+  base_scan_filtered:
+    sensor_frame: base_laser_link
+    topic: /base_scan_filtered
+    data_type: PointCloud2
+    expected_update_rate: 0.2
+    observation_persistence: 1.0
+    marking: true
+    clearing: true
+    min_obstacle_height: 0.25
+    max_obstacle_height: 0.35
+    obstacle_range: 5.0
+    raytrace_range: 3.0
+  tilt_scan_filtered:
+    sensor_frame: laser_tilt_link
+    topic: /tilt_scan_filtered
+    data_type: PointCloud2
+    expected_update_rate: 0.25
+    observation_persistence: 2.5
+    marking: true
+    clearing: true
+    min_obstacle_height: 0.075
+    max_obstacle_height: 2.0
+    obstacle_range: 5.0
+    raytrace_range: 3.0