Enable publishing camera image maintaining publishing people pose

jsk_unitree_robot/cross/install.sh

@@ -119,6 +119,7 @@ function copy_data () {
 
 if [[ ${TYPE} == "Pro" ]] ; then
     copy_data pi 192.168.123.161
+    copy_data unitree 192.168.123.13
     copy_data unitree 192.168.123.14
     ## copy_data unitree 192.168.123.15 : Pro : No Space for auto start
 elif [[ ${TYPE} == "Air" ]] ; then
@@ -129,8 +130,10 @@ else
 fi
 
 if [[ "${TARGET_DIRECTORY}" == "User" ]]; then
+    cuda_ip="192.168.123.13"
     if [[ ${TYPE} == "Pro" ]] ; then
-        cuda_ip="192.168.123.15"
+        sshpass -p 123 scp ${TARGET_MACHINE}_${TARGET_DIRECTORY}/src/jsk_robot/jsk_unitree_robot/jsk_unitree_startup/autostart/imageai.sh [email protected]:/home/unitree/Unitree/autostart/imageai/imageai.sh
+        sshpass -p 123 scp ${TARGET_MACHINE}_${TARGET_DIRECTORY}/src/jsk_robot/jsk_unitree_robot/jsk_unitree_startup/autostart/imageai.sh [email protected]:/home/unitree/Unitree/autostart/imageai/imageai.sh
     elif [[ ${TYPE} == "Air" ]] ; then
         cuda_ip="192.168.123.13"
     fi
@@ -142,8 +145,13 @@ if [[ "${TARGET_DIRECTORY}" == "User" ]]; then
     sshpass -p 123 scp ${TARGET_MACHINE}_${TARGET_DIRECTORY}/src/jsk_robot/jsk_unitree_robot/jsk_unitree_startup/scripts/publish_human_pose.diff unitree@${cuda_ip}:/tmp/publish_human_pose.diff
     sshpass -p 123 ssh -t unitree@${cuda_ip} bash -c 'ls; OUT="$(patch -p0 --backup --forward /home/unitree/Unitree/autostart/imageai/mLComSystemFrame/pyScripts/live_human_pose.py < /tmp/publish_human_pose.diff | tee /dev/tty)" || echo "${OUT}" | grep "Skipping patch" -q || (echo "$OUT" && false);'
 
-    if [[ ${TYPE} == "Air" ]] ; then
-        sshpass -p 123 ssh -t unitree@${cuda_ip} "sed -i 's/192.168.123.15/192.168.123.13/g' /home/unitree/Unitree/autostart/imageai/mLComSystemFrame/pyScripts/live_human_pose.py"
+    # launch live_human_pose.py on jetson nano (192.168.123.13)
+    sshpass -p 123 ssh -t unitree@${cuda_ip} "sed -i 's/192.168.123.15/192.168.123.13/g' /home/unitree/Unitree/autostart/imageai/mLComSystemFrame/pyScripts/live_human_pose.py"
+    # replace 192.168.123.15 -> 192.168.123.13 because live_human_pose.py is running on jetson nano (192.168.123.13)
+    if [[ ${TYPE} == "Pro" ]] ; then
+        sshpass -p 123 ssh -t [email protected] "sed -i 's/192.168.123.15/192.168.123.13/g' /home/unitree/Unitree/autostart/imageai/mLComSystemFrame/config/mqSNNRConfig.yaml"
+        sshpass -p 123 ssh -t [email protected] "sed -i 's/192.168.123.15/192.168.123.13/g' /home/unitree/Unitree/autostart/imageai/mLComSystemFrame/config/mqSNNLConfig.yaml"
+        sshpass -p 123 ssh -t [email protected] "sed -i 's/192.168.123.15/192.168.123.13/g' /home/unitree/Unitree/autostart/imageai/mLComSystemFrame/config/mqMNConfig.yaml"
     fi
 fi
 

jsk_unitree_robot/jsk_unitree_startup/autostart/imageai.sh

@@ -0,0 +1,73 @@
+#!/bin/bash
+Nano1="192.168.123.13"  ## SecNanoRight(1-2)
+Nano2="192.168.123.14"	## SecNanoLeft(3-4)
+Nano3="192.168.123.15"	## MasterNano(5)
+eval echo "[imageai] starting ... " $toStartlog
+
+mLRootPATH="/home/unitree/Unitree/autostart/imageai/"
+updatePackagePATH="/home/unitree/mLComSystemFrame.tar.gz"
+
+echo -e "\e[1;32m**** 1. Check if the program needs to be updated ? ****\e[0m"
+checkIFUpdate(){
+	if [ -f "$updatePackagePATH" ];then
+		echo -e "\e[1;32m	The update file exists, it will be updating soon ...\e[0m"
+		tar -zxvf $updatePackagePATH -C $mLRootPATH > /dev/null; sleep 1
+		rm -rf $updatePackagePATH
+		echo -e "\e[1;32m	Updating Success.\e[0m"
+	else
+		echo -e "\e[1;31m	NO Need to update!\e[0m"
+	fi
+}
+
+checkIFUpdate
+# dd=$?
+
+#declare -i lossNano1=-1
+#declare -i lossNano2=-1
+#declare -i lossNano3=-1
+
+#echo -e "\e[1;32m**** 2. Check if all Nano'IP Address is OK ? ****\e[0m"
+
+#until (( lossNano1 + lossNano2 + lossNano3 == 0 ))
+#do
+#	lossNano1=`ping -c 2 -w 2 $Nano1 | grep loss | awk '{print $6}' | awk -F "%" '{print $1}'`
+#	echo $lossNano1
+#
+#	lossNano2=`ping -c 2 -w 2 $Nano2 | grep loss | awk '{print $6}' | awk -F "%" '{print $1}'`
+#	echo $lossNano2
+#
+#	lossNano3=`ping -c 2 -w 2 $Nano3 | grep loss | awk '{print $6}' | awk -F "%" '{print $1}'`
+#	echo $lossNano3
+#	#sleep 10
+#done
+
+#echo "	IP is Right!"
+
+## GET PC's Address
+localIPAddr=`ifconfig -a|grep inet|grep -v 127.0.0.1|grep -v inet6| head -n 1 | awk '{print $2}'|tr -d "addr:"`
+echo "Local Address: "$localIPAddr
+
+echo -e "\e[1;32m**** 2. Start Main Application ... ****\e[0m"
+if [ $localIPAddr == $Nano3 ];then
+	echo "	MasterNano"
+	############### MasterNano doing things!!!!
+	gnome-terminal -- bash -c "export GST_PLUGIN_PATH=/home/unitree/Unitree/autostart/imageai/mLComSystemFrame/ThirdParty/webSinkPipe/build; cd $mLRootPATH/mLComSystemFrame/bin; ./mqttControlNode ../config/mqMNConfig.yaml; exec bash"
+	# gnome-terminal -- bash -c "export GST_PLUGIN_PATH=/home/unitree/Unitree/autostart/imageai/mLComSystemFrame/ThirdParty/webSinkPipe/build; cd $mLRootPATH/mLComSystemFrame/pyScripts; python3 live_human_pose.py; exec bash"
+
+elif [ $localIPAddr == $Nano2 ];then
+	echo "	SecNanoLeft"
+	############### SecNanoLeft(3-4) doing things!!!!
+	gnome-terminal -- bash -c "export GST_PLUGIN_PATH=/home/unitree/Unitree/autostart/imageai/mLComSystemFrame/ThirdParty/webSinkPipe/build; cd $mLRootPATH/mLComSystemFrame/bin; ./mqttControlNode ../config/mqSNNLConfig.yaml; exec bash"
+	gnome-terminal -- bash -c "export GST_PLUGIN_PATH=/home/unitree/Unitree/autostart/imageai/mLComSystemFrame/ThirdParty/webSinkPipe/build; cd $mLRootPATH/mLComSystemFrame/bin; ./mqttControlNode ../config/mqSNNLConfig.yaml 1; exec bash"
+
+elif [ $localIPAddr == $Nano1 ];then
+	echo "	SecNanoRight"
+	############### SecNanoRight(1-2) doing things!!!!
+	gnome-terminal -- bash -c "export GST_PLUGIN_PATH=/home/unitree/Unitree/autostart/imageai/mLComSystemFrame/ThirdParty/webSinkPipe/build; cd $mLRootPATH/mLComSystemFrame/bin; ./mqttControlNode ../config/mqSNNRConfig.yaml; exec bash"
+	gnome-terminal -- bash -c "export GST_PLUGIN_PATH=/home/unitree/Unitree/autostart/imageai/mLComSystemFrame/ThirdParty/webSinkPipe/build; cd $mLRootPATH/mLComSystemFrame/bin; ./mqttControlNode ../config/mqSNNRConfig.yaml 1; exec bash"
+  gnome-terminal -- bash -c "export GST_PLUGIN_PATH=/home/unitree/Unitree/autostart/imageai/mLComSystemFrame/ThirdParty/webSinkPipe/build; cd $mLRootPATH/mLComSystemFrame/pyScripts; python3 live_human_pose.py; exec bash"
+else
+	echo "	Not Found $localIPAddr in IP-Clump!"
+fi
+
+echo "	EveryThing is done!"

jsk_unitree_robot/jsk_unitree_startup/autostart/jsk_startup.sh

@@ -20,6 +20,10 @@ if [ "$ROS_IP" == "192.168.123.161" ];then
     roslaunch --screen respeaker_ros sample_respeaker.launch language:=ja-JP publish_tf:=false launch_soundplay:=false &
 fi
 
+if [ "$ROS_IP" == "192.168.123.13" ];then
+    roslaunch jsk_unitree_startup camera_image_publisher.launch &
+fi
+
 if [ "$ROS_IP" == "192.168.123.14" ];then
     # 192.168.123.14 is force updated within install.sh for Go1 Air
     if [ "$ROS_IP" == "192.168.123.13" ];then

jsk_unitree_robot/jsk_unitree_startup/launch/camera_image_publisher.launch

@@ -0,0 +1,9 @@
+<launch>
+
+  <node name="front_camera"
+        pkg="jsk_unitree_startup" type="camera_image_publisher.py"
+        output="screen"
+        respawn="true" >
+  </node>
+
+</launch>

jsk_unitree_robot/jsk_unitree_startup/scripts/camera_image_publisher.py

@@ -0,0 +1,86 @@
+#!/usr/bin/env python
+
+import base64
+
+import cv2
+import rospy
+import numpy as np
+from sensor_msgs.msg import Image
+from sensor_msgs.msg import CompressedImage
+import cv_bridge
+
+from paho.mqtt import client as mqtt_client
+
+
+def decode_image_cv2(b64encoded):
+    bin = b64encoded.split(",")[-1]
+    bin = base64.b64decode(bin)
+    bin = np.frombuffer(bin, np.uint8)
+    img = cv2.imdecode(bin, cv2.IMREAD_COLOR)
+    return img
+
+
+class ImagePublisher(object):
+    broker = '192.168.123.161'
+    port = 1883
+    topic = "vision/front_camera"
+
+    def __init__(self):
+        self.bridge = cv_bridge.CvBridge()
+        self.encoding = rospy.get_param('~encoding', 'bgr8')
+        self.frame_id = rospy.get_param('~frame_id', 'camera')
+        self.pub = rospy.Publisher('~output', Image, queue_size=1)
+        self.pub_compressed = rospy.Publisher(
+            '{}/compressed'.format(rospy.resolve_name('~output')),
+            CompressedImage, queue_size=1)
+
+        self.connect_mqtt()
+        self.subscribe()
+        self.client.loop_start()
+
+    def connect_mqtt(self):
+        def on_connect(client, userdata, flags, rc):
+            if rc == 0:
+                print("Connected to MQTT Broker! ({}:{})".format(self.broker, self.port))
+            else:
+                print("Failed to connect, return code %d\n", rc)
+        self.client = mqtt_client.Client(rospy.get_name())
+        self.client.on_connect = on_connect
+        self.client.connect(self.broker, self.port)
+        return
+
+    def subscribe(self):
+        def on_message(client, userdata, msg):
+            rospy.loginfo("Received `{}` topic".format(msg.topic))
+
+            if self.pub.get_num_connections() == 0 and self.pub_compressed.get_num_connections() == 0:
+                return
+            now = rospy.Time.now()
+            frame = decode_image_cv2(msg.payload.decode('ascii'))
+            if self.pub.get_num_connections() > 0:
+                img_msg = self.bridge.cv2_to_imgmsg(
+                    frame, encoding=self.encoding)
+                img_msg.header.frame_id = self.frame_id
+                img_msg.header.stamp = now
+                self.pub.publish(img_msg)
+            if self.pub_compressed.get_num_connections() > 0:
+                compressed_msg = CompressedImage()
+                # compressed format is separated by ';'.
+                # https://github.com/ros-perception/image_transport_plugins/blob/f0afd122ed9a66ff3362dc7937e6d465e3c3ccf7/compressed_image_transport/src/compressed_publisher.cpp#L116-L128
+                compressed_msg.format = '{}; {} compressed {}'.format(
+                    self.encoding, 'jpg', 'bgr8')
+                compressed_msg.data = np.array(
+                    cv2.imencode('.jpg', frame)[1]).tostring()
+                compressed_msg.header.frame_id = self.frame_id
+                compressed_msg.header.stamp = now
+                self.pub_compressed.publish(compressed_msg)
+
+        self.client.subscribe(self.topic)
+        self.client.on_message = on_message
+        return
+
+
+if __name__ == '__main__':
+    rospy.init_node('camera_image_publisher')
+    ImagePublisher()
+    rospy.spin()

jsk_unitree_robot/jsk_unitree_startup/scripts/publish_human_pose.diff

@@ -1,6 +1,12 @@
 --- live_human_pose.py	2022-05-26 00:17:04.634784206 +0900
 +++ live_human_pose_jsk.py	2022-05-26 00:17:18.102931810 +0900
-@@ -169,6 +169,8 @@
+@@ -1,4 +1,5 @@
+ #coding:utf-8
++import base64
+ import json
+ import trt_pose . coco
+ import trt_pose . models
+@@ -169,6 +171,8 @@
      if 39 - 39: o00ooo0 - II111iiii * OoO0O00 % o0oOOo0O0Ooo * II111iiii % II111iiii
      cv2 . circle ( src , ( i1I1iI , o0O ) , 8 , I1i1I1II , - 1 )
   iI1Ii11111iIi . write ( src )
@@ -9,3 +15,12 @@
  # cv2 . imshow ( "ai" , src )
  # cv2 . waitKey ( 1 )
   if 59 - 59: iIii1I11I1II1 + I1IiiI - o0oOOo0O0Ooo - I1IiiI + Oo / I1ii11iIi11i
+@@ -199,6 +203,8 @@
+  print ( "AI is working ...." , camIndex . value )
+  Oo0oOOo , Oo0OoO00oOO0o = o0OOO [ camIndex . value - 1 ] . read ( )
+  OOO00O = time . time ( )
++ # add by iory 2022.8.6
++ O0ii1ii1ii.publish("vision/front_camera", base64.b64encode(cv2.imencode('.jpg', Oo0OoO00oOO0o, [int(cv2.IMWRITE_JPEG_QUALITY), 90])[1]).decode('ascii'))
+  OOoOO0oo0ooO = cv2 . resize ( Oo0OoO00oOO0o , dsize = ( OooO0 , II11iiii1Ii ) , interpolation = cv2 . INTER_AREA )
+  iI ( OOoOO0oo0ooO , Oo0OoO00oOO0o , OOO00O )
+  if 98 - 98: I1II1 * I1II1 / I1II1 + O00ooOO