Merge branch 'master' into develop/pr2

.github/workflows/config.yml

@@ -38,11 +38,16 @@ jobs:
           - ROS_DISTRO: noetic
             CONTAINER: jskrobotics/ros-ubuntu:20.04-pcl
             EXTRA_DEB : "python3-rospkg python3-rospkg-modules"  #  drc_task_common need rospkg, but not installed
+            USE_DEB : false
 
     container: ${{ matrix.CONTAINER }}
     steps:
       - name: Install latest git ( use sudo for ros-ubuntu )
         run: |
+          # https://github.com/osrf/docker_images/issues/697#issuecomment-1819626877
+          sudo apt-key del 4B63CF8FDE49746E98FA01DDAD19BAB3CBF125EA || echo "OK"
+          sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-keys 4B63CF8FDE49746E98FA01DDAD19BAB3CBF125EA || echo "OK"
+          sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-keys AD19BAB3CBF125EA || echo "OK"
           [ -e /etc/apt/sources.list.d/ubuntu-esm-infra-$(lsb_release -cs).list ] && sudo rm /etc/apt/sources.list.d/ubuntu-esm-infra-$(lsb_release -cs).list ## fix Err https://esm.ubuntu.com trusty-infra-security/main amd64 Packages, gnutls_handshake() failed: Handshake failed
           (apt-get update && apt-get install -y sudo) || echo "OK"
           sudo apt-get update

.travis.rosinstall.noetic

@@ -0,0 +1,5 @@
+# this should be removed after updating .rosinstall for each robot
+- git:
+    local-name: jsk_planning
+    uri: https://github.com/jsk-ros-pkg/jsk_planning.git
+    version: master

elevator_move_base_pr2/src/matchtemplate.py

@@ -65,8 +65,8 @@ def process_msg ():
 
     try:
         cv_image = imgmsg_to_cv(msg, "mono8")
-    except CvBridgeError, e:
-        print e
+    except CvBridgeError as e:
+        print(e)
 
     reslist = []
     for template in templates:
@@ -94,7 +94,7 @@ def process_msg ():
             reslist += [(tempname,(status[1],status[3],tempthre,found))]
             if found :
                 result.data += tempname+' '
-        print reslist
+        print(reslist)
 
     result_pub.publish(result)
     publish_debug(cv_image, reslist)
@@ -120,7 +120,7 @@ def publish_debug(img, results):
         #cv.PutText(output, tempname, (0,size[1]-16), font, cv.CV_RGB(255,255,255))
         #cv.PutText(output, str(tempthre)+'<'+str(status[1]), (0,size[1]-8), font, cv.CV_RGB(255,255,255))
         for _,status in [s for s in results if s[0] == template[3]]:
-            print status
+            print(status)
             cv.PutText(output, template[3], (0,size[1]-42), font, cv.CV_RGB(255,255,255))
             cv.PutText(output, "%7.5f"%(status[0]), (0,size[1]-24), font, cv.CV_RGB(255,255,255))
             cv.PutText(output, "%7.5f"%(status[2]), (0,size[1]-8), font, cv.CV_RGB(255,255,255))

elevator_move_base_pr2/test/test-color-point-detector.py

@@ -10,7 +10,7 @@
 ## A sample python unit test
 class TestColorPointDetector(unittest.TestCase):
     def callback(self, msg):
-        print "msg.data = ", msg.data, " should be 1.5"
+        print("msg.data = ", msg.data, " should be 1.5")
         self.assert_( abs( msg.data - 1.5 ) <  0.1  )
 
     def test_light_button(self):

jsk_2013_04_pr2_610/euslisp/put-cloth-into-laundry.l

@@ -10,6 +10,21 @@
 (load "package://jsk_demo_common/euslisp/pr2-action.l")
 (load "package://jsk_2013_04_pr2_610/euslisp/objectdetection.l")
 
+;; use simple boundingbox for :gripper :link, to avoid 'error: float vector expected in (convex-hull-3d ...'
+(unless (assoc :gripper-org (send pr2-robot :methods))
+  (rplaca (assoc :gripper (send pr2-robot :methods)) :gripper-org))
+(defmethod pr2-robot
+  (:gripper
+   (limb &rest args)
+   (cond
+    ((memq :links args)
+     (let (vert-list)
+       (setq vert-list (list (flatten (send-all (flatten (send-all (send self :gripper-org limb :links) :bodies)) :vertices))))
+       (mapcar #'(lambda (v) (instance bodyset-link :init (make-cascoords) :bodies (list (send (make-bounding-box v) :body)))) vert-list)
+       ))
+    (t
+     (send* self :gripper-org limb args)
+    ))))
 (ros::roseus "laundry_button_marker_publisher")
 (ros::advertise *pub-laundry-button-topic* visualization_msgs::Marker 5)
 
@@ -332,4 +347,4 @@
 (defun push-button-laundry ()
   (warn "There is Nothing to do at func:push-button-laundry")
   t
-  )
+  )

jsk_2015_06_hrp_drc/drc_task_common/scripts/b_control_client.py

@@ -193,7 +193,7 @@ def b_control_joy_cb(msg):
         # erase_all_marker()
         try:
             ik_arm = get_ik_arm_srv().ik_arm
-        except rospy.ServiceException, e:
+        except rospy.ServiceException as e:
             ik_arm = ":rarm"
         end_effector_pose=Pose(orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0))
         if (ik_arm==":rarm"):
@@ -368,8 +368,8 @@ def selected_box_cb(msg, points_msg):
         set_dim_srv(dimensions=resp1.dim)
         rospy.loginfo("icp succeeded")
         return
-    except rospy.ServiceException, e:
-        print "ICP Service call failed: %s"%e
+    except rospy.ServiceException as e:
+        print("ICP Service call failed: %s"%e)
     # pose
     selected_only_box_cb(msg)
 def marker_info_cb(msg):
@@ -465,15 +465,15 @@ def erase_all_marker_cb(req):
 def insert_marker(shape_type=TransformableMarkerOperate.BOX, name='default_name', description='default_description', mesh_resource='', mesh_use_embedded_materials=False):
     try:
         req_marker_operate_srv(TransformableMarkerOperate(type=shape_type, action=TransformableMarkerOperate.INSERT, frame_id=default_frame_id, name=name, description=description, mesh_resource=mesh_resource, mesh_use_embedded_materials=mesh_use_embedded_materials))
-    except rospy.ServiceException, e:
-        print 'insert_marker service call failed: %s'%e
+    except rospy.ServiceException as e:
+        print('insert_marker service call failed: %s'%e)
 
 
 def erase_all_marker():
     try:
         req_marker_operate_srv(TransformableMarkerOperate(type=TransformableMarkerOperate.BOX, action=TransformableMarkerOperate.ERASEALL))
-    except rospy.ServiceException, e:
-        print 'insert_marker service call failed: %s'%e
+    except rospy.ServiceException as e:
+        print('insert_marker service call failed: %s'%e)
 
 if __name__ == '__main__':
     b_control_client_init()

jsk_2015_06_hrp_drc/drc_task_common/scripts/b_control_dummy_server.py

@@ -49,7 +49,7 @@ def reconfigure_callback(self, config, level):
         return config
 
     def pub_joy(self):
-        print self.joy
+        print(self.joy)
         self.joy_pub.publish(self.joy)
     def u1_cb(self, req):
         self.joy.axes[8] = reverse_f(self.joy.axes[8])

jsk_2015_06_hrp_drc/drc_task_common/scripts/calc_drive_tf.py

@@ -35,8 +35,8 @@ def pose_cb(pose_msg):
 
     try:
         transed_pose = listener.transformPose("BODY", pose_msg)
-    except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException), e:
-        print "tf error: %s" % e
+    except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
+        print("tf error: %s" % e)
         return
     trans.header = transed_pose.header
     trans.child_frame_id = "handle"

jsk_2015_06_hrp_drc/drc_task_common/scripts/fft_data.py

@@ -22,7 +22,7 @@
 def save_cb(float_msg):
     global flug
     if flug:
-        print "save cb driven %s" % flug
+        print("save cb driven %s" % flug)
         spamWriter.writerow([flug]+list(float_msg.data))
         flug = None
 if __name__ == "__main__":
@@ -35,7 +35,7 @@ def save_cb(float_msg):
         if(input_string == "exit"):
             exit()
         if(not input_string.isdigit()):
-            print "input num only"
+            print("input num only")
             next
         flug = input_string
         time.sleep(1)

jsk_2015_06_hrp_drc/drc_task_common/scripts/fft_decision.py

@@ -21,13 +21,13 @@ def load_data(filename):
         Y.append([float(x) for x in row[1:]])
     return (X, Y)
 def disc_cb(msg):
-    print clf.predict(msg.data)
+    print(clf.predict(msg.data))
 if __name__ == "__main__":
     rospy.init_node('fft_data', anonymous=True)
     x, y = load_data("fft_data.csv")
-    print "tests"
-    print x
-    print y
+    print("tests")
+    print(x)
+    print(y)
     clf = svm.SVC(kernel="rbf")
     clf.fit(y, x)
     rospy.Subscriber("input", Float32ArrayStamped, disc_cb)

jsk_2015_06_hrp_drc/drc_task_common/scripts/front_plane_extractor.py

@@ -8,7 +8,7 @@
 import tf
 
 def callback(polygon_msg, coeff_msg):
-    print "callback"
+    print("callback")
     # odom->ground
     max_val = -1.0;
     max_index = None

jsk_2015_06_hrp_drc/drc_task_common/scripts/gen_hosts.py

@@ -3,7 +3,7 @@
 import sys
 
 def usage():
-    print "gen_hosts.py TEAM_NO(13) MY_HOSTNAME(fc20)"
+    print("gen_hosts.py TEAM_NO(13) MY_HOSTNAME(fc20)")
 
 def gen_hosts(team_no, my_hostname):
     hosts_str ="""
@@ -21,7 +21,7 @@ def gen_hosts(team_no, my_hostname):
     ocs_hostnames = "\n".join(["10.%s.2.%s ocs%s" % (team_no, i, i)
                                for i in range(10, 255)
                                if not "ocs%s" % i == my_hostname])
-    print hosts_str % (my_hostname, fc_hostnames, ocs_hostnames)
+    print(hosts_str % (my_hostname, fc_hostnames, ocs_hostnames))
 
 
 if __name__ == "__main__":

jsk_2015_06_hrp_drc/drc_task_common/scripts/hand_box_publisher.py

@@ -25,8 +25,8 @@
         try:
             listener.waitForTransform('left_camera_optical_frame', pose_stamped.header.frame_id,  now, rospy.Duration(1))
             transed_pose = listener.transformPose('left_camera_optical_frame', pose_stamped)
-        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException, tf.Exception), e:
-            print "tf error: %s" % e
+        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException, tf.Exception) as e:
+            print("tf error: %s" % e)
             r.sleep()
             continue
         box = BoundingBox(Header(stamp=rospy.Time.now(), frame_id='left_camera_optical_frame'), transed_pose.pose, Vector3(0.15, 0.15, 0.32))

jsk_2015_06_hrp_drc/drc_task_common/scripts/imprecise_scheduler.py

@@ -80,7 +80,7 @@ def error_func(steps):
     initial_speed_factor = 5
     steps = args.distance / 0.2
     distance_factor = error_func(steps)
-    print >> sys.stderr, "distance_factor:", distance_factor
+    print("distance_factor:", distance_factor, file=sys.stderr)
     p0_table = QualityTable("q_{p0}", "package://drc_task_common/profile_data/recognition/valve_detection.csv", args.p0 * distance_factor)
     m0_table = QualityTable("q_{m0}", "package://drc_task_common/profile_data/motion/jaxon_valve_ik_stand2_average_mono.csv", args.m0 * distance_factor)
     m0_all_table = QualityTable("q_{m0}", "package://drc_task_common/profile_data/motion/jaxon_valve_ik_stand2_average.csv", args.m0 * distance_factor)
@@ -101,7 +101,7 @@ def error_func(steps):
     args.m0 = 0.1
     args.p1 = 1.0
     args.m1 = 1.0
-    print >> sys.stderr, "distance_factor:", distance_factor
+    print("distance_factor:", distance_factor, file=sys.stderr)
     p0_table = QualityTable("q_{p0}", "package://drc_task_common/profile_data/recognition/valve_detection.csv", args.p0 * distance_factor)
     p1_table = QualityTable("q_{p1}", "package://drc_task_common/profile_data/recognition/valve_detection.csv", 1.0)
     m0_table = QualityTable("q_{m0}", "package://drc_task_common/profile_data/motion/jaxon_valve_ik_stand2_average_mono.csv", args.m0 * distance_factor)
@@ -129,7 +129,7 @@ def error_func(steps):
     args.m0 = 0.2
     args.p1 = 1.0
     args.m1 = 0.5
-    print >> sys.stderr, "distance_factor:", distance_factor
+    print("distance_factor:", distance_factor, file=sys.stderr)
     p0_table = QualityTable("q_{p0}", "package://drc_task_common/profile_data/recognition/door_detection.csv", args.p0 * distance_factor)
     p1_table = QualityTable("q_{p1}", "package://drc_task_common/profile_data/recognition/door_detection.csv", 1.0)
     m0_table = QualityTable("q_{m0}", "package://drc_task_common/profile_data/motion/jaxon_door_ik_stand2_average_mono.csv", args.m0 * distance_factor)
@@ -153,7 +153,7 @@ def error_func(steps):
     initial_speed_factor = 5
     steps = args.distance / 0.2
     distance_factor = error_func(steps)
-    print >> sys.stderr, "distance_factor:", distance_factor
+    print("distance_factor:", distance_factor, file=sys.stderr)
     p0_table = QualityTable("q_{p0}", "package://drc_task_common/profile_data/recognition/door_detection.csv", args.p0 * distance_factor)
     m0_table = QualityTable("q_{m0}", "package://drc_task_common/profile_data/motion/jaxon_door_ik_stand2_average_mono.csv", args.m0 * distance_factor)
     m0_all_table = QualityTable("q_{m0}", "package://drc_task_common/profile_data/motion/jaxon_door_ik_stand2_average.csv", args.m0 * distance_factor)
@@ -262,7 +262,7 @@ def error_func(steps):
         break
     container.draw(ax)
     if args.save_img:
-        print 'image_{0:0>3}.png'.format(counter)
+        print('image_{0:0>3}.png'.format(counter))
         plt.savefig('image_{0:0>3}.png'.format(counter))
     counter = counter + 1
     if args.incremental:

jsk_2015_06_hrp_drc/drc_task_common/scripts/joy_to_twist.py

@@ -56,7 +56,7 @@ def joy_cb(msg):
             factor_flag_b = True
             if factor < 0.6:
                 factor*=1.2
-        print "factor: %f" % factor
+        print("factor: %f" % factor)
     else:
         factor_flag_b = False
 
@@ -65,7 +65,7 @@ def joy_cb(msg):
             factor_flag_s = True
             if factor > 0.01:
                 factor/=1.2
-        print "factor: %f" % factor
+        print("factor: %f" % factor)
     else:
         factor_flag_s = False
     if msg.buttons[8]:

jsk_2015_06_hrp_drc/drc_task_common/scripts/move_marker_with_point.py

@@ -17,19 +17,19 @@
 def point_cb(point_msg):
     try:
         pose = get_pose("").pose_stamped
-    except rospy.ServiceException, e:
-        print "Service fail: %s" % e
+    except rospy.ServiceException as e:
+        print("Service fail: %s" % e)
         return
     try:
         transed_point = listener.transformPoint(pose.header.frame_id, point_msg)
         pose.pose.position = transed_point.point
-    except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException), e:
-        print "tf error: %s" % e
+    except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
+        print("tf error: %s" % e)
         return
     try:
         set_pose(target_name="", pose_stamped=pose)
-    except rospy.ServiceException, e:
-        print "Service fail: %s" % e
+    except rospy.ServiceException as e:
+        print("Service fail: %s" % e)
         return
 
 if __name__ == "__main__":

jsk_2015_06_hrp_drc/drc_task_common/scripts/ocs_reconfigure.py

@@ -5,4 +5,4 @@
 from drc_task_common.cfg import DRCParametersConfig
 
 for (name, type) in DRCParametersConfig.type.items():
-  print name, type
+  print(name, type)

jsk_2015_06_hrp_drc/drc_task_common/scripts/request_handle_marker.py

@@ -20,15 +20,15 @@ def request_handle_marker():
     handle_request = rospy.ServiceProxy('/drive/handle_server/request_marker_operate', RequestMarkerOperate)
     try:
         handle_request(jsk_rviz_plugins.msg.TransformableMarkerOperate(2, 0, "BODY", "handle_marker", ""))
-    except rospy.ServiceException, e:
-        print "Service fail: %s" % e
+    except rospy.ServiceException as e:
+        print("Service fail: %s" % e)
         return
     rospy.wait_for_service("/drive/handle_server/set_dimensions")
     handle_size_request = rospy.ServiceProxy('/drive/handle_server/set_dimensions', SetMarkerDimensions)
     try:
         handle_size_request("", jsk_interactive_marker.msg.MarkerDimensions(0, 0, 0, 0.14, 0.01, 0))
-    except rospy.ServiceException, e:
-        print "Service fail: %s" % e
+    except rospy.ServiceException as e:
+        print("Service fail: %s" % e)
         return
 
 

jsk_2015_06_hrp_drc/drc_task_common/scripts/vehicle/Magnetometer2Direction.py

@@ -19,7 +19,7 @@ def __init__(self):
             try:
                 self.offset_ang = float(self.argv[1]) * pi / 180
             except:
-                print "DATA(1st Argument) Should Be Float, So Default (0.0) Is Used."
+                print("DATA(1st Argument) Should Be Float, So Default (0.0) Is Used.")
                 self.offset_ang = 0.0
         else:
             self.offset_ang = 0.0

jsk_2015_06_hrp_drc/drc_task_common/scripts/vehicle/car_path_visualizer.py

@@ -41,8 +41,8 @@ def execute(self):
                         self.R = quaternion_matrix(quat)[0:3,0:3]
                         # print self.R
                         self.marker_publisher()
-                    except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException), e:
-                        print "tf error: %s" % e
+                    except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
+                        print("tf error: %s" % e)
                         pass
             self.r.sleep()