diff --git a/jsk_unitree_robot/cross/install.sh b/jsk_unitree_robot/cross/install.sh index e05fa2f36f..a2d184c6ee 100755 --- a/jsk_unitree_robot/cross/install.sh +++ b/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 unitree@192.168.123.13:/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 unitree@192.168.123.15:/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 unitree@192.168.123.13 "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 unitree@192.168.123.14 "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 unitree@192.168.123.15 "sed -i 's/192.168.123.15/192.168.123.13/g' /home/unitree/Unitree/autostart/imageai/mLComSystemFrame/config/mqMNConfig.yaml" fi fi diff --git a/jsk_unitree_robot/jsk_unitree_startup/autostart/imageai.sh b/jsk_unitree_robot/jsk_unitree_startup/autostart/imageai.sh new file mode 100755 index 0000000000..6f98fd581b --- /dev/null +++ b/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!" diff --git a/jsk_unitree_robot/jsk_unitree_startup/autostart/jsk_startup.sh b/jsk_unitree_robot/jsk_unitree_startup/autostart/jsk_startup.sh index 4f508dca41..0e96a8e217 100755 --- a/jsk_unitree_robot/jsk_unitree_startup/autostart/jsk_startup.sh +++ b/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 diff --git a/jsk_unitree_robot/jsk_unitree_startup/launch/camera_image_publisher.launch b/jsk_unitree_robot/jsk_unitree_startup/launch/camera_image_publisher.launch new file mode 100644 index 0000000000..942961102c --- /dev/null +++ b/jsk_unitree_robot/jsk_unitree_startup/launch/camera_image_publisher.launch @@ -0,0 +1,9 @@ + + + + + + diff --git a/jsk_unitree_robot/jsk_unitree_startup/scripts/camera_image_publisher.py b/jsk_unitree_robot/jsk_unitree_startup/scripts/camera_image_publisher.py new file mode 100755 index 0000000000..ca1d079204 --- /dev/null +++ b/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() diff --git a/jsk_unitree_robot/jsk_unitree_startup/scripts/publish_human_pose.diff b/jsk_unitree_robot/jsk_unitree_startup/scripts/publish_human_pose.diff index 960c221f2f..cfae1f90d7 100644 --- a/jsk_unitree_robot/jsk_unitree_startup/scripts/publish_human_pose.diff +++ b/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