Merge branch 'master' into fetch-fix-dock

jsk_baxter_robot/jsk_baxter_startup/baxter.launch

@@ -7,14 +7,14 @@
   <arg name="launch_voice_echo" default="true"/>
   <arg name="launch_moveit" default="true"/>
   <arg name="launch_teleop" default="false"/>
-  <arg name="launch_db" default="$(arg launch_mongodb)"
-       doc="launch jsk_robot_lifelog logging"/>
   <arg name="launch_mongodb" default="false"
        doc="Deprecated. Please use launch_db instead."/>
-  <arg name="launch_twitter" default="$(arg launch_tweet)"
-       doc="launch twitter" />
+  <arg name="launch_db" default="$(arg launch_mongodb)"
+       doc="launch jsk_robot_lifelog logging"/>
   <arg name="launch_tweet" default="false"
        doc="Deprecated. Please use launch_twitter instead." />
+  <arg name="launch_twitter" default="$(arg launch_tweet)"
+       doc="launch twitter" />
   <arg name="launch_wrench" default="true"/>
   <arg name="launch_time_signal" default="true"/>
   <arg name="sanity_check_joint_trajectory" default="true" />

jsk_fetch_robot/fetcheus/fetch-interface.l

@@ -13,8 +13,8 @@
   )
 
 (defmethod fetch-interface
-  (:init (&rest args)
-   (prog1 (send-super* :init :robot fetch-robot :base-frame-id "/base_link" :odom-topic "/odom_combined" :base-controller-action-name nil args)
+  (:init (&key (default-collision-object t) &rest args)
+   (prog1 (send-super* :init :robot fetch-robot :base-frame-id "base_link" :odom-topic "/odom_combined" :base-controller-action-name nil args)
      (send self :add-controller :arm-controller)
      (send self :add-controller :torso-controller)
      (send self :add-controller :head-controller)
@@ -29,10 +29,18 @@
                      :groupname groupname))
      (setq moveit-robot (instance fetch-robot :init))
      (send self :set-moveit-environment (instance fetch-moveit-environment :init :robot moveit-robot))
+     (when (and (boundp '*co*) default-collision-object)
+       (send self :delete-keepout-collision-object)
+       (send self :delete-ground-collision-object)
+       (send self :add-keepout-collision-object)
+       (send self :add-ground-collision-object))
      ))
   (:state (&rest args)
    ":state calls with :wait-until-update by default, since Fetch publishes /joint_states from body and gripper at almost same frequency"
-   (send-super* :state (if (member :wait-until-update args) args (append args (list :wait-until-update t)))))
+   (send-super* :state
+                (if (or (eq (car args) :worldcoords)
+                        (member :wait-until-update args))
+                    args (append args (list :wait-until-update t)))))
   (:check-continuous-joint-move-over-180 ;; can be removed if http//github.com/jsk-ros-pkg/jsk_pr2eus/pull/322 merged
    (diff-av)
    (let ((i 0) add-new-trajectory-point)
@@ -191,7 +199,26 @@ Example: (send self :gripper :position) => 0.00"
     (send-super* :speak-en text :topic-name topic-name args))
   (:speak-jp (text &rest args &key (topic-name "robotsound_jp") &allow-other-keys)
     (send-super* :speak-jp text :topic-name topic-name args))
-  )
+  (:add-keepout-collision-object ()
+    (let ((cube (make-cube 200 350 50))
+          (keepout (make-cylinder 300 50)))
+      (send cube :translate #f(-120 0 0))
+      (send keepout :translate #f(0 0 -25))
+      (setq keepout (body- keepout cube))
+      (send keepout :translate #f(0 0 375))
+      (send *co* :add-object keepout :frame-id base-frame-id :object-id "keepout")))
+  (:delete-keepout-collision-object ()
+    (send *co* :delete-object-by-id "keepout"))
+  (:add-ground-collision-object ()
+    (let ((cube (make-cube 200 350 50))
+          (ground (make-cylinder 1000 50)))
+      (send cube :translate #f(-120 0 0))
+      (send ground :translate #f(0 0 -25))
+      (setq ground (body- ground cube))
+      (send ground :translate #f(0 0 -25))
+      (send *co* :add-object ground :frame-id base-frame-id :object-id "ground")))
+  (:delete-ground-collision-object ()
+    (send *co* :delete-object-by-id "ground")))
 
 ;; interface for simple base actions
 (defmethod fetch-interface
@@ -229,9 +256,9 @@ Example: (send self :gripper :position) => 0.00"
     (ros::rate 10)
     t)
   (:go-pos-unsafe
-    (&rest args)
-    (send self :put :go-pos-unsafe-no-wait-goal (coerce args float-vector))
-    (send self :go-pos-unsafe-wait)
+    (x y d &rest args)
+    (send self :put :go-pos-unsafe-no-wait-goal (float-vector x y d))
+    (send* self :go-pos-unsafe-wait args)
     t)
   (:go-pos-unsafe-no-wait
     (x y &optional (d 0)) ;; [m] [m] [degree]
@@ -256,7 +283,7 @@ Example: (send self :gripper :position) => 0.00"
              (forward-p (>= x 0))
              (d0 (if forward-p (atan2 y x) (atan2 (- y) (- x))))
              (d1 (distance (float-vector 0 0) (float-vector x y)))
-             (d2 (shortest-angle (- d d0) 0))
+             (d2 (shortest-angle (- (deg2rad d) d0) 0))
              org-cds cur-cds diffrot diffpos err)
         (send self :remprop :go-pos-unsafe-no-wait-goal)
 
@@ -329,11 +356,14 @@ Example: (send self :gripper :position) => 0.00"
                 d-vel (* (if (>= d-vel 0) 1.0 -1.0) min-rotation-abs-vel))))
           (ros::sleep))
         t)))
+  (:move-to-wait (&rest args &key (correction nil) &allow-other-keys)
+                 (send-super* :move-to-wait :correction correction args))
 ) ;; fetch-interface (simple base actions)
 
 (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 '*ri*) (setq *ri* (instance fetch-interface :init)))
 
   (ros::spin-once)

jsk_fetch_robot/fetcheus/test/test-fetch-moveit.l

@@ -9,6 +9,11 @@
 (setq *fetch* (fetch))
 (send *fetch* :reset-pose)
 
+
+(deftest test-fetch-state-worldcoords ()
+  (let ((coords (send *ri* :state :worldcoords "base_link")))
+    (assert coords (format nil "failed to get :state :worldcoords" coords))))
+
 (deftest test-fetch-moveit ()
   (let (tm-0 tm-1)
     (setq tm-0 (ros::time-now))

jsk_fetch_robot/jsk_fetch_diagnosis/CMakeLists.txt

@@ -23,7 +23,7 @@ catkin_package(
   CATKIN_DEPENDS message_runtime
 )
 
-install(DIRECTORY scripts
+install(DIRECTORY node_scripts
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
   USE_SOURCE_PERMISSIONS
 )

jsk_fetch_robot/jsk_fetch_startup/CMakeLists.txt

@@ -24,6 +24,10 @@ catkin_add_env_hooks(99.jsk_fetch_startup SHELLS bash zsh sh
 ## Install ##
 #############
 install(DIRECTORY config launch scripts data
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  USE_SOURCE_PERMISSIONS)
+
+install(FILES jsk_fetch.machine
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
 
 #############

jsk_fetch_robot/jsk_fetch_startup/launch/fetch_sensors.xml

@@ -97,6 +97,14 @@
         <remap from="~output" to="throttled/image_rect_color" />
         <param name="update_rate" value="$(arg throttled_rate)" />
       </node>
+      <node name="throttle_rgb_compressed"
+            pkg="nodelet" type="nodelet"
+            args="load jsk_topic_tools/LightweightThrottle /$(arg manager)"
+            respawn="true">
+        <remap from="~input" to="image_rect_color/compressed" />
+        <remap from="~output" to="throttled/image_rect_color/compressed" />
+        <param name="update_rate" value="$(arg throttled_rate)" />
+      </node>
       <node name="downsample_half"
             pkg="nodelet" type="nodelet"
             args="load image_proc/resize /$(arg manager)"

jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_warning/pr2_reset_motors.py

@@ -35,13 +35,15 @@ def motors_cb(self, msg):
         halted = msg.data
         rospy.logdebug("runstop: %s, halted: %s" % (self.run_stop, halted))
         if self.run_stop and halted:
+            rospy.logwarn("motor halted, but run stop is true")
             stamp = rospy.Time.now()
             history = filter(
                 lambda s: (stamp-s).to_sec() < self.watch_duration,
                 self.history)
             if len(history) > self.max_retry_num:
                 rospy.logerr("Maximum retry count reached. Give up resetting motors")
                 return
+            rospy.logwarn("resetting motors")
             self.reset_srv()
             self.history = history + [stamp]
 

jsk_robot_common/jsk_robot_startup/CMakeLists.txt

@@ -99,7 +99,7 @@ install(DIRECTORY lifelog util launch images config cfg
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
   USE_SOURCE_PERMISSIONS)
 
-install(TARGETS jsk_robot_lifelog ${${PROJET_NAME}_LIFELOG_NODELET_EXECUTABLES}
+install(TARGETS jsk_robot_lifelog joint_states_throttle ${${PROJET_NAME}_LIFELOG_NODELET_EXECUTABLES}
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}

jsk_robot_common/jsk_robot_startup/lifelog/common_logger.launch

@@ -44,11 +44,14 @@
   <arg name="respawn" default="false" />
   <arg name="vital_check" default="true" />
   <arg name="vital_rate" default="0.1" />
+  <arg name="debug" default="false" />
+  <arg     if="$(arg debug)" name="launch-prefix" default="gdb -ex run --args" />
+  <arg unless="$(arg debug)" name="launch-prefix" default="" />
 
   <!-- nodelet manager -->
   <node name="$(arg manager)" if="$(arg launch_manager)"
         pkg="nodelet" type="nodelet" args="manager" machine="$(arg machine)"
-        output="screen" respawn="$(arg respawn)"/>
+        output="screen" respawn="$(arg respawn)" launch-prefix="$(arg launch-prefix)"/>
 
   <group ns="lifelog">
     <!-- image throttle -->
@@ -57,7 +60,7 @@
         <node name="camera_throttle"
               pkg="nodelet" type="nodelet"
               args="load jsk_topic_tools/SynchronizedThrottle /$(arg manager)"
-              machine="$(arg machine)"
+              machine="$(arg machine)" launch-prefix="$(arg launch-prefix)"
               respawn="$(arg respawn)">
           <remap from="/$(arg camera_ns)/$(arg rgb_ns)/$(arg camera_info_topic)/throttled"
                  to="rgb/$(arg camera_info_topic)"/>
@@ -82,7 +85,7 @@
         <node name="camera_throttle"
               pkg="nodelet" type="nodelet"
               args="load jsk_topic_tools/SynchronizedThrottle /$(arg manager)"
-              machine="$(arg machine)"
+              machine="$(arg machine)" launch-prefix="$(arg launch-prefix)"
               respawn="$(arg respawn)">
           <remap from="/$(arg camera_ns)/$(arg rgb_ns)/$(arg camera_info_topic)/throttled"
                  to="rgb/$(arg camera_info_topic)"/>
@@ -103,7 +106,7 @@
         <node name="camera_throttle"
               pkg="nodelet" type="nodelet"
               args="load jsk_topic_tools/SynchronizedThrottle /$(arg manager)"
-              machine="$(arg machine)"
+              machine="$(arg machine)" launch-prefix="$(arg launch-prefix)"
               respawn="$(arg respawn)">
           <remap from="/$(arg camera_ns)/$(arg depth_ns)/$(arg camera_info_topic)/throttled"
                  to="depth/$(arg camera_info_topic)"/>
@@ -125,7 +128,7 @@
       <node name="rgb_image_logger"
             pkg="nodelet" type="nodelet"
             args="load jsk_robot_lifelog/LightweightLogger /$(arg manager)"
-            machine="$(arg machine)"
+            machine="$(arg machine)" launch-prefix="$(arg launch-prefix)"
             respawn="$(arg respawn)">
         <remap from="~input" to="rgb/$(arg rgb_topic)$(arg rgb_suffix)" />
         <rosparam subst_value="true">
@@ -137,7 +140,7 @@
       <node name="rgb_camera_info_logger"
             pkg="nodelet" type="nodelet"
             args="load jsk_robot_lifelog/LightweightLogger /$(arg manager)"
-            machine="$(arg machine)"
+            machine="$(arg machine)" launch-prefix="$(arg launch-prefix)"
             respawn="$(arg respawn)">
         <remap from="~input" to="rgb/$(arg camera_info_topic)" />
         <rosparam subst_value="true">
@@ -151,7 +154,7 @@
       <node name="depth_image_logger"
             pkg="nodelet" type="nodelet"
             args="load jsk_robot_lifelog/LightweightLogger /$(arg manager)"
-            machine="$(arg machine)"
+            machine="$(arg machine)" launch-prefix="$(arg launch-prefix)"
             respawn="$(arg respawn)">
         <remap from="~input" to="depth/$(arg depth_topic)$(arg depth_suffix)" />
         <rosparam subst_value="true">
@@ -163,7 +166,7 @@
       <node name="depth_camera_info_logger"
             pkg="nodelet" type="nodelet"
             args="load jsk_robot_lifelog/LightweightLogger /$(arg manager)"
-            machine="$(arg machine)"
+            machine="$(arg machine)" launch-prefix="$(arg launch-prefix)"
             respawn="$(arg respawn)">
         <remap from="~input" to="depth/$(arg camera_info_topic)" />
         <rosparam subst_value="true">
@@ -189,7 +192,7 @@
     <group if="$(arg save_joint_states)">
       <node name="joint_states_throttle"
             pkg="jsk_robot_startup" type="joint_states_throttle"
-            machine="$(arg machine)"
+            machine="$(arg machine)" launch-prefix="$(arg launch-prefix)"
             respawn="$(arg respawn)">
         <remap from="~input" to="/$(arg joint_states_topic)" />
         <rosparam subst_value="true">
@@ -198,7 +201,7 @@
       </node>
       <node name="joint_states_logger"
             pkg="jsk_robot_startup" type="lightweight_logger"
-            machine="$(arg machine)"
+            machine="$(arg machine)" launch-prefix="$(arg launch-prefix)"
             respawn="$(arg respawn)">
         <remap from="~input" to="joint_states_throttle/output" />
         <rosparam subst_value="true">