Enable noetic install and fix bug in baxter HMD teleoperation

eus_teleop/README.md

@@ -25,15 +25,11 @@ This package depends on these branches below:
 
 ### Build environment
 
-#### Ubuntu 16.04 + ROS Kinetic
+#### Ubuntu 20.04 + ROS Noetic
 
-- NVidia driver: `396.37`
-- OpenVR: `1.3.22`
-- Steam VR: `1.6.10`
-
-#### Ubuntu 18.04 + ROS Melodic
+Steam does not work on Melodic or Kinetic now.
 
-- NVidia driver: `390.116`
+- NVidia driver: `525.105`
 - OpenVR: `1.3.22`
 - Steam VR: `1.6.10`
 
@@ -92,6 +88,12 @@ sudo apt install nvidia-396
 
 Follow [here](https://github.com/knorth55/vive_ros)
 
+#### Remove libcurl packages in Steam (for Vive)
+
+```bash
+rm -r ~/.steam/steam/ubuntu12_32/steam-runtime/usr/lib/x86_64-linux-gnu/libcurl*
+```
+
 #### Install kodak 4k pro camera and ELP usb camera udev (for Baxter)
 
 ```bash
@@ -252,13 +254,29 @@ https://user-images.githubusercontent.com/9300063/213144997-0d224d5c-b462-43a7-b
 
 #### Baxter + Vive in JSK 73B2
 
+##### Baxter control PC
+
 ```bash
 rossetip
 rossetmaster baxter
-# HMD mode
-roslaunch eus_teleop baxter_vive.launch
+roslaunch eus_teleop baxter_remote.launch
+```
+
+##### Vive PC
+
+```bash
+rossetip
+rossetmaster baxter
+
 # No HMD mode
-roslaunch eus_teleop baxter_vive.launch head:=false
+roslaunch eus_teleop baxter_vive_remote.launch
+# for display
+roslaunch eus_teleop baxter_display_remote.launch
+
+# HMD mode
+roslaunch eus_teleop baxter_vive_remote.launch head:=true
+# for display
+roslaunch eus_teleop baxter_display_remote.launch head:=true
 ```
 
 #### Baxter + Vive in Gazebo

eus_teleop/baxter.rosinstall.noetic

@@ -0,0 +1,4 @@
+- git:
+    local-name: strands-project/mongodb_store
+    uri: https://github.com/strands-project/mongodb_store.git
+    version: noetic-devel

eus_teleop/euslisp/lib/baxter-teleop-interface.l

@@ -70,7 +70,9 @@
     (let (coords)
       (send tfl :wait-for-transform base-frame-id head-frame-id (ros::time 0) 0.1)
       (setq coords (send tfl :lookup-transform base-frame-id head-frame-id (ros::time 0)))
-      (if coords (send coords :rotate pi/2 :y :world))
+      (if coords
+        (setq coords (make-coords :pos (send coords :worldpos)
+                                  :rpy (float-vector 0 pi/2 0))))
       coords))
   (:angle-vector (&rest args)
     (send* robot-interface :angle-vector-raw args))

eus_teleop/euslisp/lib/baxter-vive-interface.l

@@ -23,11 +23,11 @@
       (send self :set-val 'menu-wait-count 5)
       (send self :set-val 'trackpad-wait-count 5)
       (send self :set-val 'grip-wait-count 5)))
-  (:get-hmd->vive-coords (arm)
-    (let* ((coords (send-super :get-hmd->vive-coords arm))
-           (bvive->hmd-coords (send self :get-bvive->hmd-coords))
+  (:get-hmd->controller-coords (arm)
+    (let* ((coords (send-super :get-hmd->controller-coords arm))
+           (vrbase->hmd-coords (send self :get-vrbase->hmd-coords))
            (rpy-angle
-             (if bvive->hmd-coords (car (send bvive->hmd-coords :rpy-angle)))))
+             (if vrbase->hmd-coords (car (send vrbase->hmd-coords :rpy-angle)))))
       (if (and coords rpy-angle)
         (progn
           (setq coords

eus_teleop/euslisp/lib/robot-move-base-vive-interface.l

@@ -14,7 +14,7 @@
 (defmethod robot-vive-interface
   ;; overwrite controller button callbacks
   (:controller-button-cb (arm msg)
-    (send :controller-button-cb-move-base-vive-org arm msg)
+    (send self :controller-button-cb-move-base-vive-org arm msg)
     (cond
       ((eq arm :larm)
        (send self :set-val 'vw

eus_teleop/euslisp/lib/robot-teleop-interface.l

@@ -708,10 +708,16 @@
              (if (or (equal button-prefix "larm-")
                      (equal button-prefix "rarm-"))
                (read-from-string
-                 (format nil "~Aprev-~A" button-prefix (subseq (format nil "~A" 5))))
+                 (format nil "~Aprev-~A" button-prefix (subseq (format nil "~A" button) 5)))
                (read-from-string (format nil "prev-~A" button))))
            (button-counter (read-from-string (format nil "~A-count" button)))
-           (button-wait-counter (read-from-string (format nil "~A-wait-count" button)))
+           (button-wait-counter
+             (read-from-string
+               (format nil "~A-wait-count"
+                       (if (or (equal button-prefix "larm-")
+                               (equal button-prefix "rarm-"))
+                         (subseq (format nil "~A" button) 5)
+                         button))))
            (button-p (send self :get-val button))
            (prev-button-p (send self :get-val prev-button))
            (button-count (send self :get-val button-counter))

eus_teleop/fc.rosinstall

@@ -28,11 +28,10 @@
     uri: https://github.com/jsk-ros-pkg/jsk_visualization.git
     version: master
 # need fixed version of rviz_camera_stream
-# https://github.com/knorth55/rviz_camera_stream/commit/ca7ffb379946710a3045dea2d191ba0218a8fd08
 - git:
     local-name: knorth55/rviz_camera_stream
     uri: https://github.com/knorth55/rviz_camera_stream
-    version: master
+    version: noetic-devel
 # use larger image for vr
 # https://github.com/knorth55/vive_ros/commit/67fa4803ac324846608b062b44c618335bfa0c45
 - git:
@@ -45,3 +44,8 @@
     local-name: UTNuclearRoboticsPublic/rviz_textured_sphere
     uri: https://github.com/knorth55/rviz_textured_sphere.git
     version: vive-control
+# for pr2_moveit_config in noetic
+- git:
+    local-name: ros-planning/moveit_pr2
+    uri: https://github.com/ros-planning/moveit_pr2.git
+    version: melodic-devel

eus_teleop/launch/pr2/pr2_teleop_display.launch

@@ -12,12 +12,11 @@
   <arg name="DECOMP_IMAGE_COLOR" default="/kinect_head_decompressed/rgb/image_rect_color" />
   <arg name="DECOMP_POINTS" default="/kinect_head_decompressed/depth_registered/points" />
 
-  <!-- pr1012 and pr1040 parameters -->
-  <arg name="CAMERA_INFO" />
-  <arg name="RGB_CAMERA_INFO" />
-  <arg name="IMAGE_COLOR" />
-  <arg name="IMAGE_DEPTH" />
-  <arg name="IMAGE_DEPTH_TYPE" />
+  <arg name="CAMERA_INFO" default="/kinect_head/depth_registered/camera_info" />
+  <arg name="RGB_CAMERA_INFO" default="/kinect_head/rgb/camera_info" />
+  <arg name="IMAGE_COLOR" default="/kinect_head/rgb/image_rect_color" />
+  <arg name="IMAGE_DEPTH" default="/kinect_head/depth_registered/image_rect" />
+  <arg name="IMAGE_DEPTH_TYPE" default="compressedDepth" />
 
   <group ns="kinect_head_decompressed">
     <node name="camera_info_relay" pkg="topic_tools" type="relay"

eus_teleop/sample/vive/baxter/baxter_vive_gazebo.launch

@@ -2,7 +2,7 @@
   <arg name="main" default="true" />
   <arg name="vive" default="true" />
   <arg name="vive_set" default="default" />
-  <arg name="head" default="true" />
+  <arg name="head" default="false" />
   <arg name="mirror" default="false" />
   <arg name="button_toggle" default="true" />
   <arg name="display" default="true" />

eus_teleop/sample/vive/baxter/baxter_vive_mirror.launch

@@ -2,7 +2,7 @@
   <arg name="main" default="true" />
   <arg name="vive" default="true" />
   <arg name="vive_set" default="default" />
-  <arg name="head" default="true" />
+  <arg name="head" default="false" />
   <arg name="respeaker" value="true" />
   <arg name="realsense_camera" default="false" />
   <arg name="spherical_camera" default="true" />

eus_teleop/sample/vive/baxter/baxter_vive_remote.launch

@@ -2,7 +2,7 @@
   <arg name="main" default="true" />
   <arg name="vive" default="true" />
   <arg name="vive_set" default="default" />
-  <arg name="head" default="true" />
+  <arg name="head" default="false" />
   <arg name="mirror" default="false" />
   <arg name="lgripper_type" default="softhand" />
   <arg name="rgripper_type" default="softhand" />