diff --git a/jsk_robot_common/jsk_robot_startup/CMakeLists.txt b/jsk_robot_common/jsk_robot_startup/CMakeLists.txt index 7f597875af..a9ca7f482e 100644 --- a/jsk_robot_common/jsk_robot_startup/CMakeLists.txt +++ b/jsk_robot_common/jsk_robot_startup/CMakeLists.txt @@ -59,6 +59,7 @@ add_message_files( FILES RoomLight.msg Email.msg + EmailBody.msg ) generate_messages( diff --git a/jsk_robot_common/jsk_robot_startup/README.md b/jsk_robot_common/jsk_robot_startup/README.md index ce43e215a6..5544d7016e 100644 --- a/jsk_robot_common/jsk_robot_startup/README.md +++ b/jsk_robot_common/jsk_robot_startup/README.md @@ -209,6 +209,59 @@ rostopic pub /reboot std_msgs/Empty ``` +## scripts/smach_to_mail.py + +This node sends smach messages to `/email`, `/tweet`, etc... to notify robot state transition. + +```mermaid +flowchart TB + subgraph S[Demo Code Running on SMACH] + START --> State_1 + State_1 --> State_2 + State_2 --> END + end + subgraph E[SMACH To Notification] + id1[(state list
DESCRIPTION / IMAGE
DESCRIPTION / IMAGE
...)] + id1 --> email[[pub /email jsk_robot_startup::Email]] + id1 --> tweet[[pub /tweet std_msgs::String]] + id1 --> chat[[pub /google_chat_ros/send/goal
google_chat_ros::SendMessageAction]] + end + S -->|"[{'DESCRIPTION': string, 'IMAGE': base64}]"| E + email --> email_body(["DESCRIPTION[0]
DESCRIPTION[1]
IMAGE[1]
DESCRIPTION[1]
IMAGE[1]
..."]) + tweet --> tweet_body1(["DESCRIPTION[0]"]) + tweet_body1 --> tweet_body2(["DESCRIPTION[1]
IMAGE[1]"]) + tweet_body2 --> tweet_body3(["DESCRIPTION[2]
IMAGE[2]"]) + chat --> chat_body1(["DESCRIPTION[0]"]) + chat_body1 --> chat_body2(["DESCRIPTION[1]
IMAGE[1]"]) + chat_body2 --> chat_body3(["DESCRIPTION[2]
IMAGE[2]"]) +``` + +### Subscribing Topics + +* `~smach/container_status` (`smach_msgs/SmachContainerStatus`) + + Input topic smach status + +### Publishing Topics + +* `/email` (`jsk_robot_startup/Email`) + + Email message with description and image + +* `/tweet` (`std_msgs/String`) + + Tweet message with description and image + +### Parameters + +* `~sender_address` (String) + + Sender address + +* `~receiver_address` (String) + + Receiver address + ## launch/safe_teleop.launch This launch file provides a set of nodes for safe teleoperation common to mobile robots. Robot-specific nodes such as `/joy`, `/teleop` or `/cable_warning` must be included in the teleop launch file for each robot, such as [safe_teleop.xml for PR2](https://github.com/jsk-ros-pkg/jsk_robot/blob/master/jsk_pr2_robot/jsk_pr2_startup/jsk_pr2_move_base/safe_teleop.xml) or [safe_teleop.xml for fetch](https://github.com/jsk-ros-pkg/jsk_robot/blob/master/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_teleop.xml). diff --git a/jsk_robot_common/jsk_robot_startup/euslisp/email-topic-client.l b/jsk_robot_common/jsk_robot_startup/euslisp/email-topic-client.l index a78d46de09..8def28a2ff 100644 --- a/jsk_robot_common/jsk_robot_startup/euslisp/email-topic-client.l +++ b/jsk_robot_common/jsk_robot_startup/euslisp/email-topic-client.l @@ -1,5 +1,7 @@ (ros::load-ros-manifest "jsk_robot_startup") +(require :base64 "lib/llib/base64.l") + (defun init-mail () (ros::advertise "email" jsk_robot_startup::Email 1) (ros::spin-once) @@ -16,7 +18,23 @@ (setq msg (instance jsk_robot_startup::Email :init)) (send msg :header :stamp (ros::time-now)) (send msg :subject subject) - (send msg :body body) + (send msg :body + (mapcar #'(lambda (e) + (cond ((derivedp e image-2d) + (if (> (length (send e :entity)) (* 8 8192)) + (warning-message 3 "The size of img is too large (~A)~%You may encounter 'too long string' error, see https://github.com/euslisp/EusLisp/issues/2 for more info~%" (length (send e :entity)))) + (instance jsk_robot_startup::EmailBody :init :type "img" + :img_data (base64encode (send e :entity)) + :img_size 100)) + ((probe-file e) + (instance jsk_robot_startup::EmailBody :init :type "img" :file_path e :img_size 100)) + ((and (> (length e) 0) ;; check html + (eq (elt e 0) #\<) + (eq (elt e (1- (length e))) #\>)) + (instance jsk_robot_startup::EmailBody :init :type "html" :message e)) + (t + (instance jsk_robot_startup::EmailBody :init :type "text" :message (format nil "~A" e))))) + (if (atom body) (list body) body))) (send msg :sender_address sender-address) (send msg :receiver_address receiver-address) (send msg :smtp_server smtp-server) diff --git a/jsk_robot_common/jsk_robot_startup/euslisp/sample-email-topic-client.l b/jsk_robot_common/jsk_robot_startup/euslisp/sample-email-topic-client.l index 855b5f9fef..ad280eca65 100755 --- a/jsk_robot_common/jsk_robot_startup/euslisp/sample-email-topic-client.l +++ b/jsk_robot_common/jsk_robot_startup/euslisp/sample-email-topic-client.l @@ -7,10 +7,36 @@ (ros::spin-once) (setq receiver-address (ros::get-param "~receiver_address")) (setq attached-files (ros::get-param "~attached_files" nil)) -(unix::sleep 10) ;; wait for server +(while (or (null (ros::get-num-subscribers "email")) + (<= (ros::get-num-subscribers "email") 0)) + (ros::ros-warn "wait for email topic") + (unix::sleep 1)) (ros::ros-info "Sending a mail to ~A" receiver-address) -(send-mail "test-mail" receiver-address "test" :attached-files attached-files) -(ros::ros-info "Sent a mail") +(send-mail "test text mail" receiver-address "test") +(ros::ros-info "Sent a text mail") +(unix:sleep 1) + +(send-mail "test html mail" receiver-address "

test with html mail

") +(ros::ros-info "Sent a html mail") +(unix:sleep 1) + +(send-mail "test attached mail" receiver-address "test with attached image" :attached-files attached-files) +(ros::ros-info "Sent a mail with attached files ~A" attached-files) +(unix:sleep 1) + +(when attached-files + (send-mail "test image embedded mail (file)" receiver-address (append attached-files (list "test with embedded image"))) + (ros::ros-info "sent a mail with embedded image ~a" attached-files) + (unix:sleep 1) + ;; read png/jpeg file, but do not extract to raw image, keep original compressed data in (img . entity) + (setq img (read-image-file (elt attached-files 0))) + (setq (img . entity) (make-string (elt (unix:stat (elt attached-files 0)) 7))) ;; size of file + (with-open-file (f (elt attached-files 0)) (unix:uread (send f :infd) (img . entity))) + (send-mail "test image embedded mail (string)" receiver-address (list "test with embedded image" img)) + (ros::ros-info "Sent a mail with embedded image ~A" img) + (unix:sleep 1) + ) + (ros::roseus "shutdown") (exit) diff --git a/jsk_robot_common/jsk_robot_startup/msg/Email.msg b/jsk_robot_common/jsk_robot_startup/msg/Email.msg index 5726aa1477..08cc108e30 100644 --- a/jsk_robot_common/jsk_robot_startup/msg/Email.msg +++ b/jsk_robot_common/jsk_robot_startup/msg/Email.msg @@ -1,6 +1,6 @@ std_msgs/Header header string subject # Email subject -string body # Email body +jsk_robot_startup/EmailBody[] body # Email body string sender_address # Sender's email address string receiver_address # receiver's email address string smtp_server # smtp server address diff --git a/jsk_robot_common/jsk_robot_startup/msg/EmailBody.msg b/jsk_robot_common/jsk_robot_startup/msg/EmailBody.msg new file mode 100644 index 0000000000..db3626ff0a --- /dev/null +++ b/jsk_robot_common/jsk_robot_startup/msg/EmailBody.msg @@ -0,0 +1,5 @@ +string type # text, html, img +string message # For text and html type +string file_path # For img type, set file path +string img_data # For img type, you can also use base64 encoded image data instead of file path +uint8 img_size # For img type [percent] diff --git a/jsk_robot_common/jsk_robot_startup/scripts/email_topic.py b/jsk_robot_common/jsk_robot_startup/scripts/email_topic.py index 9918e174d8..4ec7f06f5b 100755 --- a/jsk_robot_common/jsk_robot_startup/scripts/email_topic.py +++ b/jsk_robot_common/jsk_robot_startup/scripts/email_topic.py @@ -1,6 +1,7 @@ #!/usr/bin/env python from email.mime.application import MIMEApplication +from email.mime.image import MIMEImage from email.mime.text import MIMEText from email.mime.multipart import MIMEMultipart import errno @@ -12,6 +13,9 @@ import socket from socket import error as socket_error import yaml +import copy +import random, string # for image cid +import base64 class EmailTopic(object): @@ -43,7 +47,11 @@ def __init__(self): 'email', Email, self._cb, queue_size=1) def _cb(self, msg): - rospy.loginfo('Received an msg: {}'.format(msg)) + msg_compact = copy.deepcopy(msg) + for content in msg_compact.body: + if len(content.img_data) >= 64: + content.img_data = content.img_data[:64] + "...." + rospy.loginfo('Received an msg: {}'.format(msg_compact)) send_mail_args = {} # Set default value for self._send_mail arguments send_mail_args['subject'] = '' @@ -75,7 +83,39 @@ def _send_mail( msg["Subject"] = subject msg["From"] = sender_address msg["To"] = receiver_address - msg.attach(MIMEText(body, 'plain', 'utf-8')) + # Support embed image + for content in body: + if content.type == 'text': + msg.attach(MIMEText(content.message, 'plain', 'utf-8')) + elif content.type == 'html': + msg.attach(MIMEText(content.message, 'html')) + elif content.type == 'img': + if content.img_data != '': + embed_img = MIMEImage(base64.b64decode(content.img_data)) + cid = ''.join(random.choice(string.ascii_lowercase) for i in range(16)) + elif os.path.exists(content.file_path): + with open(content.file_path, 'rb') as img: + embed_img = MIMEImage(img.read()) + cid = content.file_path + else: + rospy.logerr("'img' content requries either file_path {}".format(content.type)) + rospy.logerr(" or img_data {} with img_format {}".format(content.img_data, content.img_format)) + continue + embed_img.add_header( + 'Content-ID', '<{}>'.format(cid)) + embed_img.add_header( + 'Content-Disposition', 'inline; filename="{}"'.format(os.path.basename(cid))) + msg.attach(embed_img) # This line is necessary to embed + if content.img_size: + image_size = content.img_size + else: + image_size = 100 + text = ''.format(cid, image_size) + bodytext = MIMEText(text, 'html') + msg.attach(bodytext) + else: + rospy.logwarn('Unknown content type {}'.format(content.type)) + continue # Attach file for attached_file in attached_files: if attached_file == '': diff --git a/jsk_robot_common/jsk_robot_startup/scripts/smach_to_mail.py b/jsk_robot_common/jsk_robot_startup/scripts/smach_to_mail.py new file mode 100755 index 0000000000..33b7f8eac6 --- /dev/null +++ b/jsk_robot_common/jsk_robot_startup/scripts/smach_to_mail.py @@ -0,0 +1,192 @@ +#!/usr/bin/env python + +import base64 +import cv2 +import datetime +import os +import pickle +import rospy +import sys +import yaml + +from cv_bridge import CvBridge +from jsk_robot_startup.msg import Email +from jsk_robot_startup.msg import EmailBody +from sensor_msgs.msg import CompressedImage +from smach_msgs.msg import SmachContainerStatus +from std_msgs.msg import String + + +class SmachToMail(): + + def __init__(self): + rospy.init_node('server_name') + # it should be 'smach_to_mail', but 'server_name' + # is the default name of smach_ros + self.pub_email = rospy.Publisher("email", Email, queue_size=10) + self.pub_twitter = rospy.Publisher("tweet", String, queue_size=10) + rospy.Subscriber( + "~smach/container_status", SmachContainerStatus, self._status_cb) + self.bridge = CvBridge() + self.smach_state_list = {} # for status list + self.smach_state_subject = {} # for status subject + yaml_path = rospy.get_param( + '~email_info', "/var/lib/robot/email_info.yaml") + if os.path.exists(yaml_path): + with open(yaml_path) as yaml_f: + self.email_info = yaml.load(yaml_f) + rospy.loginfo( + "{} is loaded as email config file.".format(yaml_path)) + rospy.loginfo(self.email_info) + self.sender_address = self.email_info['sender_address'] + self.receiver_address = self.email_info['receiver_address'] + else: + if rospy.has_param("~sender_address"): + self.sender_address = rospy.get_param("~sender_address") + else: + rospy.logerr("Please set rosparam {}/sender_address".format( + rospy.get_name())) + if rospy.has_param("~receiver_address"): + self.receiver_address = rospy.get_param("~receiver_address") + else: + rospy.logerr("Please set rosparam {}/receiver_address".format( + rospy.get_name())) + + + def _status_cb(self, msg): + ''' + Recording starts when smach_state becomes start. + ''' + rospy.loginfo("Received SMACH status") + if len(msg.active_states) == 0: + return + + # Print received messages + status_str = ', '.join(msg.active_states) + if sys.version_info.major < 3: + local_data_str = pickle.loads(msg.local_data) + else: + local_data_str = pickle.loads( + msg.local_data.encode('utf-8'), encoding='utf-8') + info_str = msg.info + if not type(local_data_str) is dict: + rospy.logerr("local_data_str:({}) is not dictionary".format(local_data_str)) + rospy.logerr("May be you forget to pass user data in exec-state-machine ?") + rospy.logerr("please check your program execute smach with (exec-state-machine (sm) '((description . "")(image . "")))") + + rospy.loginfo("- status -> {}".format(status_str)) + rospy.loginfo("- info_str -> {}".format(info_str)) + if 'DESCRIPTION' in local_data_str: + rospy.loginfo("- description_str -> {}".format(local_data_str['DESCRIPTION'])) + else: + rospy.logwarn("smach does not have DESCRIPTION, see https://github.com/jsk-ros-pkg/jsk_robot/tree/master/jsk_robot_common/jsk_robot_startup#smach_to_mailpy for more info") + if 'IMAGE' in local_data_str and local_data_str['IMAGE']: + rospy.loginfo("- image_str -> {}".format(local_data_str['IMAGE'][:64])) + + # Store data for every callerid to self.smach_state_list[caller_id] + caller_id = msg._connection_header['callerid'] + + # If we received START/INIT status, restart storing smach_state_list + if status_str in ["START", "INIT"]: + self.smach_state_list[caller_id] = [] + # DESCRIPTION of START is MAIL SUBJECT + if 'DESCRIPTION' in local_data_str: + self.smach_state_subject[caller_id] = local_data_str['DESCRIPTION'] + del local_data_str['DESCRIPTION'] + else: + self.smach_state_subject[caller_id] = None + + # Build status_dict for every status + # expected keys are 'DESCRIPTION' , 'IMAGE', 'STATE', 'INFO', 'TIME' + status_dict = {} + if 'DESCRIPTION' in local_data_str: + status_dict.update({'DESCRIPTION': local_data_str['DESCRIPTION']}) + + if 'IMAGE' in local_data_str and local_data_str['IMAGE']: + imgmsg = CompressedImage() + imgmsg.deserialize(base64.b64decode(local_data_str['IMAGE'])) + cv_image = self.bridge.compressed_imgmsg_to_cv2(imgmsg, "bgr8") + scale_percent = 640.0 / cv_image.shape[1] * 100.0 + width = int(cv_image.shape[1] * scale_percent / 100) + height = int(cv_image.shape[0] * scale_percent / 100) + dim = (width, height) + cv_image = cv2.resize(cv_image, dim, interpolation = cv2.INTER_AREA) + status_dict.update({'IMAGE': base64.b64encode(cv2.imencode('.jpg', cv_image)[1].tostring())}) # dict is complicated? + status_dict.update({'STATE': status_str}) + status_dict.update({'INFO': info_str}) + status_dict.update({'TIME': datetime.datetime.fromtimestamp(msg.header.stamp.to_sec())}) + + + if (caller_id not in self.smach_state_list) or self.smach_state_list[caller_id] is None: + rospy.logwarn("received {}, but we did not find START node".format(status_dict)) + else: + self.smach_state_list[caller_id].append(status_dict) + + # If we received END/FINISH status, send email, etc... + if status_str in ["END", "FINISH"]: + if (caller_id not in self.smach_state_list) or self.smach_state_list[caller_id] is None: + rospy.logwarn("received END node, but we did not find START node") + rospy.logwarn("failed to send {}".format(status_dict)) + else: + rospy.loginfo("END!!") + rospy.loginfo("Following SMACH is reported") + for x in self.smach_state_list[caller_id]: + rospy.loginfo(" - At {}, Active state is {}{}".format(x['TIME'], x['STATE'], + "({})".format(x['INFO']) if x['INFO'] else '')) + self._send_mail(self.smach_state_subject[caller_id], self.smach_state_list[caller_id]) + self._send_twitter(self.smach_state_subject[caller_id], self.smach_state_list[caller_id]) + self.smach_state_list[caller_id] = None + + def _send_mail(self, subject, state_list): + email_msg = Email() + email_msg.body = [] + changeline = EmailBody() + changeline.type = 'html' + changeline.message = "
" + for x in state_list: + if 'DESCRIPTION' in x: + description = EmailBody() + description.type = 'text' + description.message = x['DESCRIPTION'] + email_msg.body.append(description) + email_msg.body.append(changeline) + if 'IMAGE' in x and x['IMAGE']: + image = EmailBody() + image.type = 'img' + image.img_size = 100 + image.img_data = x['IMAGE'] + email_msg.body.append(image) + email_msg.body.append(changeline) + # rospy.loginfo("body:{}".format(email_msg.body)) + + if subject: + email_msg.subject = subject + else: + email_msg.subject = 'Message from {}'.format(rospy.get_param('/robot/name', 'robot')) + + email_msg.sender_address = self.sender_address + email_msg.receiver_address = self.receiver_address + + rospy.loginfo("send '{}' email to {}".format(email_msg.subject, email_msg.receiver_address)) + + self.pub_email.publish(email_msg) + + def _send_twitter(self, subject, state_list): + text = "" + if subject: + text += subject + for x in state_list: + if 'DESCRIPTION' in x and x['DESCRIPTION']: + text += '\n' + x['DESCRIPTION'] + if 'IMAGE' in x and x['IMAGE']: + img_txt = x['IMAGE'] + if isinstance(img_txt, bytes): + img_txt = img_txt.decode('ascii') + text += img_txt + if len(text) > 1: + self.pub_twitter.publish(String(text)) + + +if __name__ == '__main__': + stm = SmachToMail() + rospy.spin() diff --git a/jsk_unitree_robot/cross/Makefile b/jsk_unitree_robot/cross/Makefile index e942246cf6..7a05f06953 100644 --- a/jsk_unitree_robot/cross/Makefile +++ b/jsk_unitree_robot/cross/Makefile @@ -20,6 +20,9 @@ user: install: ./install.sh +compress: + ./compress.sh + clean: rm -fr ${TARGET_MACHINE}_ws_* diff --git a/jsk_unitree_robot/cross/README.md b/jsk_unitree_robot/cross/README.md index 4c38e5c985..a15b6f05c4 100644 --- a/jsk_unitree_robot/cross/README.md +++ b/jsk_unitree_robot/cross/README.md @@ -14,9 +14,9 @@ $ sudo usermod -aG docker $USER $ newgrp ``` -2. Install Qemu software +2. Install Qemu software and other prerequisites ``` -$ sudo apt install -y qemu-user-static +$ sudo apt install -y qemu-user-static sshpass python-vcstool ``` ### Build ROS System on Docker (Run only the fist time per host computer) @@ -29,7 +29,7 @@ Caution!!! It will take more than a few hours !! So for JSK users, download the Run following command to copy ROS1 base sytem to Go1 onboard computer. This should be done only in the first time. So normally user do not have to run this command ``` -./install.sh -p 123 -D System +./install.sh -p 123 -d System ``` ### Build `jsk_unitree_robot` software on Docker diff --git a/jsk_unitree_robot/cross/build_ros1.sh b/jsk_unitree_robot/cross/build_ros1.sh index 05712d2b1d..31548745c5 100755 --- a/jsk_unitree_robot/cross/build_ros1.sh +++ b/jsk_unitree_robot/cross/build_ros1.sh @@ -4,6 +4,7 @@ TARGET_MACHINE="${TARGET_MACHINE:-arm64v8}" HOST_INSTALL_ROOT="${BASE_ROOT:-${PWD}}/"${TARGET_MACHINE}_System INSTALL_ROOT=System SOURCE_ROOT=${TARGET_MACHINE}_ws_system +MAKEFLAGS=${MAKEFLAGS:-'-j4'} UPDATE_SOURCE_ROOT=1 # TRUE if [ -e "${SOURCE_ROOT}" ]; then @@ -32,7 +33,9 @@ mkdir -p ${HOST_INSTALL_ROOT}/ros1_inst if [ ${UPDATE_SOURCE_ROOT} -eq 1 ]; then vcs import --force --retry 3 --shallow ${SOURCE_ROOT}/src < repos/roseus_no_window.repos for dir in euslisp jskeus; do ls ${SOURCE_ROOT}/src/$dir/patches/; rsync -avz ${SOURCE_ROOT}/src/$dir/patches/ ${SOURCE_ROOT}/src/$dir; done - sed -i s@:{version}@0.0.0@ ${SOURCE_ROOT}/src/euslisp/package.xml ${SOURCE_ROOT}/src/jskeus/package.xml + # linux can use sed -i'.bak' and latest mac also supports same syntax. + # https://stackoverflow.com/questions/4247068/sed-command-with-i-option-failing-on-mac-but-works-on-linux/14813278#14813278 + sed -i.bak s@:{version}@0.0.0@ ${SOURCE_ROOT}/src/euslisp/package.xml ${SOURCE_ROOT}/src/jskeus/package.xml fi wget https://patch-diff.githubusercontent.com/raw/PR2/pr2_mechanism/pull/346.diff -O ${SOURCE_ROOT}/pr2_mechanism-346.diff @@ -45,11 +48,22 @@ JSK_ROBOT_UTILS="jsk_network_tools" DIAGNOSTIC_AGGREGATOR="diagnostic_aggregator" # jsk_XXX_startup usually depends on diagnostic_aggregator PR2EUS="pr2eus" +case ${OSTYPE} in + linux*) + OPTIONS="-u $(id -u $USER)" + ;; + darwin*) + OPTIONS="" + ;; +esac + docker run -it --rm \ - -u $(id -u $USER) \ + ${OPTIONS} \ -e INSTALL_ROOT=${INSTALL_ROOT} \ + -e MAKEFLAGS=${MAKEFLAGS} \ -v ${HOST_INSTALL_ROOT}/ros1_dependencies:/opt/jsk/${INSTALL_ROOT}/ros1_dependencies:ro \ -v ${HOST_INSTALL_ROOT}/ros1_dependencies_setup.bash:/opt/jsk/${INSTALL_ROOT}/ros1_dependencies_setup.bash:ro \ + -v ${PWD}/startup_scripts/sitecustomize.py:/usr/lib/python2.7/sitecustomize.py:ro \ -v ${HOST_INSTALL_ROOT}/ros1_inst:/opt/jsk/${INSTALL_ROOT}/ros1_inst:rw \ -v ${PWD}/${SOURCE_ROOT}:/home/user/${SOURCE_ROOT}:rw \ ros1-unitree:${TARGET_MACHINE} \ @@ -69,3 +83,4 @@ docker run -it --rm \ " 2>&1 | tee ${TARGET_MACHINE}_build_ros1.log cp ${PWD}/startup_scripts/system_setup.bash ${HOST_INSTALL_ROOT}/ +cp ${PWD}/startup_scripts/sitecustomize.py ${HOST_INSTALL_ROOT}/ diff --git a/jsk_unitree_robot/cross/build_ros1_dependencies.sh b/jsk_unitree_robot/cross/build_ros1_dependencies.sh index 56b8f6a418..9f51f7663c 100755 --- a/jsk_unitree_robot/cross/build_ros1_dependencies.sh +++ b/jsk_unitree_robot/cross/build_ros1_dependencies.sh @@ -4,6 +4,7 @@ TARGET_MACHINE="${TARGET_MACHINE:-arm64v8}" HOST_INSTALL_ROOT="${BASE_ROOT:-${PWD}}/"${TARGET_MACHINE}_System INSTALL_ROOT=System SOURCE_ROOT=${TARGET_MACHINE}_ws_ros1_dependencies_sources +MAKEFLAGS=${MAKEFLAGS:-'-j4'} if [ -e "${SOURCE_ROOT}" ]; then echo "WARNING: Source directory is found ${SOURCE_ROOT}" 1>&2 @@ -24,10 +25,21 @@ cp repos/ros1_dependencies.repos ${SOURCE_ROOT}/ mkdir -p ${HOST_INSTALL_ROOT}/Python cp repos/go1_requirements.txt ${SOURCE_ROOT}/go1_requirements.txt +cp repos/go1_requirements_python3.txt ${SOURCE_ROOT}/go1_requirements_python3.txt + +case ${OSTYPE} in + linux*) + OPTIONS="-u $(id -u $USER)" + ;; + darwin*) + OPTIONS="" + ;; +esac docker run -it --rm \ - -u $(id -u $USER) \ + ${OPTIONS} \ -e INSTALL_ROOT=${INSTALL_ROOT} \ + -e MAKEFLAGS=${MAKEFLAGS} \ -v ${PWD}/ros1_dependencies_build_scripts:/home/user/ros1_dependencies_build_scripts:ro \ -v ${PWD}/${SOURCE_ROOT}:/home/user/ros1_dependencies_sources:rw \ -v ${HOST_INSTALL_ROOT}/ros1_dependencies:/opt/jsk//${INSTALL_ROOT}/ros1_dependencies:rw \ @@ -40,8 +52,9 @@ docker run -it --rm \ for script_file in \$(ls /home/user/ros1_dependencies_build_scripts/|sort); do /home/user/ros1_dependencies_build_scripts/\$script_file || exit 1; done && \ + pip3 install -U --user pip && \ pip install -U --user pip && \ - export PYTHONPATH=\"/opt/jsk/System/ros1_dependencies/lib/python2.7/site-packages\" && \ export PKG_CONFIG_PATH=\"/opt/jsk/${INSTALL_ROOT}/ros1_dependencies/lib/pkgconfig\" && \ - ~/.local/bin/pip install --prefix=/opt/jsk/${INSTALL_ROOT}/Python -r /home/user/ros1_dependencies_sources/go1_requirements.txt \ + ~/.local/bin/pip install --prefix=/opt/jsk/${INSTALL_ROOT}/Python -r /home/user/ros1_dependencies_sources/go1_requirements.txt && \ + PYTHONPATH= ~/.local/bin/pip3 install --prefix=/opt/jsk/${INSTALL_ROOT}/Python -r /home/user/ros1_dependencies_sources/go1_requirements_python3.txt \ " 2>&1 | tee ${TARGET_MACHINE}_build_ros1_dependencies.log diff --git a/jsk_unitree_robot/cross/build_user.sh b/jsk_unitree_robot/cross/build_user.sh index f18831c411..bf47b79068 100755 --- a/jsk_unitree_robot/cross/build_user.sh +++ b/jsk_unitree_robot/cross/build_user.sh @@ -41,15 +41,26 @@ done # See https://github.com/k-okada/jsk_robot/issues/61 docker run -it --rm ros1-unitree:${TARGET_MACHINE} bash -c 'exit' || docker run --rm --privileged multiarch/qemu-user-static --reset -p yes +case ${OSTYPE} in + linux*) + OPTIONS="-u $(id -u $USER)" + ;; + darwin*) + OPTIONS="" + ;; +esac + # run on docker docker run -it --rm \ - -u $(id -u $USER) \ + ${OPTIONS} \ -e INSTALL_ROOT=${INSTALL_ROOT} \ -v ${HOST_INSTALL_ROOT}/ros1_dependencies:/opt/jsk/${INSTALL_ROOT}/ros1_dependencies:ro \ -v ${HOST_INSTALL_ROOT}/Python:/opt/jsk/${INSTALL_ROOT}/Python:ro \ -v ${HOST_INSTALL_ROOT}/ros1_inst:/opt/jsk/${INSTALL_ROOT}/ros1_inst:ro \ -v ${HOST_INSTALL_ROOT}/ros1_dependencies_setup.bash:/opt/jsk/${INSTALL_ROOT}/ros1_dependencies_setup.bash:ro \ -v ${HOST_INSTALL_ROOT}/system_setup.bash:/opt/jsk/${INSTALL_ROOT}/system_setup.bash:ro \ + -v ${HOST_INSTALL_ROOT}/sitecustomize.py:/usr/lib/python2.7/sitecustomize.py:ro \ + -v ${HOST_INSTALL_ROOT}/sitecustomize.py:/usr/lib/python3.6/sitecustomize.py:ro \ -v ${PWD}/${SOURCE_ROOT}:/opt/jsk/User:rw \ -v ${PWD}/rosinstall_generator_unreleased.py:/home/user/rosinstall_generator_unreleased.py:ro \ ros1-unitree:${TARGET_MACHINE} \ @@ -59,8 +70,9 @@ docker run -it --rm \ set -xeuf -o pipefail && \ cd /opt/jsk/User && \ [ ${UPDATE_SOURCE_ROOT} -eq 0 ] || ROS_PACKAGE_PATH=src:\${ROS_PACKAGE_PATH} /home/user/rosinstall_generator_unreleased.py jsk_${TARGET_ROBOT}_startup ${TARGET_ROBOT}eus --rosdistro melodic --exclude RPP --exclude mongodb_store | tee user.repos && \ - [ ${UPDATE_SOURCE_ROOT} -eq 0 -o -z \"\$(cat user.repos)\" ] || PYTHONPATH= vcs import src < user.repos && \ + [ ${UPDATE_SOURCE_ROOT} -eq 0 -o -z \"\$(cat user.repos)\" ] || vcs import src < user.repos && \ catkin build jsk_${TARGET_ROBOT}_startup ${TARGET_ROBOT}eus -s -vi \ --cmake-args -DCATKIN_ENABLE_TESTING=FALSE \ " 2>&1 | tee ${TARGET_MACHINE}_build_user.log cp ${PWD}/startup_scripts/user_setup.bash ${SOURCE_ROOT}/ +cp ${PWD}/startup_scripts/env.sh ${SOURCE_ROOT}/ diff --git a/jsk_unitree_robot/cross/compress.sh b/jsk_unitree_robot/cross/compress.sh new file mode 100755 index 0000000000..fd8a79096b --- /dev/null +++ b/jsk_unitree_robot/cross/compress.sh @@ -0,0 +1,29 @@ +#!/bin/bash + +if [ -e "arm64v8_User" ]; then + if [ -e "arm64v8_User.tar.gz" ]; then + echo "WARNING: Compressed arm64v8_User.tar.gz is found." + read -p "WARNING: Are you sure to continue [y/N] ? " -n 1 -r + echo # (optional) move to a new line + if [[ $REPLY =~ ^[Yy]$ ]]; then + tar -zcvf arm64v8_User.tar.gz arm64v8_User + fi + else + tar -zcvf arm64v8_User.tar.gz arm64v8_User + fi +fi + +if [ -e "arm64v8_System" ]; then + if [ -e "arm64v8_System.tar.gz" ]; then + echo "WARNING: Compressed arm64v8_System.tar.gz is found." + read -p "WARNING: Are you sure to continue [y/N] ? " -n 1 -r + echo # (optional) move to a new line + if [[ $REPLY =~ ^[Yy]$ ]]; then + chmod 644 arm64v8_System/ros1_inst/share/pr2eus/*.l + tar -zcvf arm64v8_System.tar.gz arm64v8_System + fi + else + chmod 644 arm64v8_System/ros1_inst/share/pr2eus/*.l + tar -zcvf arm64v8_System.tar.gz arm64v8_System + fi +fi diff --git a/jsk_unitree_robot/cross/install.sh b/jsk_unitree_robot/cross/install.sh index e05fa2f36f..85029d4d77 100755 --- a/jsk_unitree_robot/cross/install.sh +++ b/jsk_unitree_robot/cross/install.sh @@ -54,7 +54,9 @@ function copy_data () { fi # Check if robot is reachable - reachability=$(ping -c4 ${hostname} 2>/dev/null | awk '/---/,0' | grep -Po '[0-9]{1,3}(?=% packet loss)') + # Use perl instead of `grep -Po '[0-9]{1,3}(?=% packet loss)'` for Mac environment. + # See https://stackoverflow.com/questions/16658333/grep-p-no-longer-works-how-can-i-rewrite-my-searches/16658690#16658690 + reachability=$(ping -c4 ${hostname} 2>/dev/null | awk '/---/,0' | perl -nle'print $& while m{[0-9]{1,3}(?=% packet loss)}g') if [ -z "$reachability" ] || [ "$reachability" == 100 ]; then echo "ERROR: ${hostname} unreachable" 1>&2 exit 2 @@ -64,6 +66,12 @@ function copy_data () { ssh-keygen -f "${HOME}/.ssh/known_hosts" -R "${hostname}" || echo "OK" sshpass -p $PASS ssh -o StrictHostKeyChecking=no ${user}@${hostname} exit + if [[ "${TARGET_DIRECTORY}" == "System" ]]; then + sshpass -p 123 scp ${TARGET_MACHINE}_${TARGET_DIRECTORY}/sitecustomize.py ${user}@${hostname}:/tmp/sitecustomize.py + sshpass -p $PASS ssh -t ${user}@${hostname} "echo $PASS | sudo -S cp -f /tmp/sitecustomize.py /usr/lib/python2.7/sitecustomize.py" + sshpass -p $PASS ssh -t ${user}@${hostname} "echo $PASS | sudo -S cp -f /tmp/sitecustomize.py /usr/lib/python3/sitecustomize.py" + fi + # cehck disk space echo "Copy ${TARGET_MACHINE}_${TARGET_DIRECTORY} ...." echo "===" @@ -79,7 +87,7 @@ function copy_data () { echo "Check user id for /dev/ttyACM0 etc" echo "===" sshpass -p $PASS ssh -t ${user}@${hostname} bash -c 'id' - sshpass -p $PASS ssh -t ${user}@${hostname} bash -c "groups | grep dialout || (set -x; sudo usermod -a -G dialout ${user})" + sshpass -p $PASS ssh -t ${user}@${hostname} bash -c "groups | grep dialout || (set -x; echo $PASS | sudo -S usermod -a -G dialout ${user})" echo "===" # check if we have jsk_startup in .startlist @@ -90,11 +98,15 @@ function copy_data () { # check if we have /opt/jsk set -x sshpass -p $PASS ssh -t ${user}@${hostname} "test -e /opt/jsk" || \ - sshpass -p $PASS ssh -t ${user}@${hostname} "sudo mkdir -p /opt/jsk && sudo chown -R \$(id -u \${USER}):\$(id -g \${USER}) /opt/jsk && ls -al /opt/jsk" + sshpass -p $PASS ssh -t ${user}@${hostname} "echo $PASS | sudo -S mkdir -p /opt/jsk && sudo chown -R \$(id -u \${USER}):\$(id -g \${USER}) /opt/jsk && ls -al /opt/jsk" - rsync --rsh="/usr/bin/sshpass -p $PASS ssh -o StrictHostKeyChecking=no -l ${user}" -avz --delete --delete-excluded --exclude "*.pyc" --exclude "^logs/" ${TARGET_MACHINE}_${TARGET_DIRECTORY}/ ${hostname}:/opt/jsk/${TARGET_DIRECTORY} + # check if we have /var/mail/${user} + sshpass -p $PASS ssh -t ${user}@${hostname} "echo $PASS | sudo -S touch /var/mail/${user}" && \ + sshpass -p $PASS ssh -t ${user}@${hostname} "echo $PASS | sudo -S sudo chown -R \$(id -u \${USER}):\$(id -g \${USER}) /var/mail/${user}" + + rsync --rsh="sshpass -p $PASS ssh -o StrictHostKeyChecking=no -l ${user}" -avz --delete --delete-excluded --exclude "*.pyc" --exclude "^logs/" ${TARGET_MACHINE}_${TARGET_DIRECTORY}/ ${hostname}:/opt/jsk/${TARGET_DIRECTORY} if [[ "${TARGET_DIRECTORY}" == "User" ]]; then - rsync --rsh="/usr/bin/sshpass -p $PASS ssh -o StrictHostKeyChecking=no -l ${user}" -avz --delete --delete-excluded ../jsk_unitree_startup/autostart/ ${hostname}:Unitree/autostart/jsk_startup + rsync --rsh="sshpass -p $PASS ssh -o StrictHostKeyChecking=no -l ${user}" -avz --delete --delete-excluded ../jsk_unitree_startup/autostart/ ${hostname}:Unitree/autostart/jsk_startup # https://stackoverflow.com/questions/23395363/make-patch-return-0-when-skipping-an-already-applied-patch # On Air, we need to start unitree_bringup at 129.168.123.13 @@ -104,21 +116,28 @@ function copy_data () { # update udev # respeaker_ros - sshpass -p $PASS ssh -t ${user}@${hostname} "source /opt/jsk/User/user_setup.bash; sudo cp -f \$(rospack find respeaker_ros)/config/60-respeaker.rules /etc/udev/rules.d/60-respeaker.rules" + sshpass -p $PASS ssh -t ${user}@${hostname} "source /opt/jsk/User/user_setup.bash; echo $PASS | sudo -S cp -f \$(rospack find respeaker_ros)/config/60-respeaker.rules /etc/udev/rules.d/60-respeaker.rules" # - sshpass -p $PASS ssh -t ${user}@${hostname} "ls -al /etc/udev/rules.d/; sudo systemctl restart udev" - fi + sshpass -p $PASS ssh -t ${user}@${hostname} "ls -al /etc/udev/rules.d/; echo $PASS | sudo -S systemctl restart udev" - # enable Internet with USB LTE module - if [[ "${hostname}" == "192.168.123.161" ]]; then - sshpass -p $PASS ssh -t ${user}@${hostname} "source /opt/jsk/User/user_setup.bash; sudo cp -f \$(rospack find jsk_unitree_startup)/config/dhcpcd.conf /etc/dhcpcd.conf" - sshpass -p $PASS ssh -t ${user}@${hostname} "sudo systemctl restart dhcpcd" + # enable Internet with USB LTE module + if [[ "${hostname}" == "192.168.123.161" ]]; then + sshpass -p $PASS ssh -t ${user}@${hostname} "source /opt/jsk/User/user_setup.bash; echo $PASS | sudo -S cp -f \$(rospack find jsk_unitree_startup)/config/dhcpcd.conf /etc/dhcpcd.conf" + sshpass -p $PASS ssh -t ${user}@${hostname} "source /opt/jsk/User/user_setup.bash; echo $PASS | sudo -S cp -f \$(rospack find jsk_unitree_startup)/config/sysctl.conf /etc/sysctl.conf" + sshpass -p $PASS ssh -t ${user}@${hostname} "source /opt/jsk/User/user_setup.bash; echo $PASS | sudo -S cp -f \$(rospack find jsk_unitree_startup)/config/iptables.ipv4.nat /etc/iptables.ipv4.nat" + sshpass -p $PASS ssh -t ${user}@${hostname} "echo $PASS | sudo -S systemctl restart dhcpcd" + + # enable wlan0 + sshpass -p $PASS ssh -t ${user}@${hostname} "sed -i 's/sudo ifconfig wlan0 down/# sudo ifconfig wlan0 down/g' /home/pi/Unitree/autostart/configNetwork/configNetwork.sh" + fi fi + set +x } 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 diff --git a/jsk_unitree_robot/cross/repos/go1_requirements_python3.txt b/jsk_unitree_robot/cross/repos/go1_requirements_python3.txt new file mode 100644 index 0000000000..4e9c52a6c8 --- /dev/null +++ b/jsk_unitree_robot/cross/repos/go1_requirements_python3.txt @@ -0,0 +1,5 @@ +# needeed by catkin_virtualenv +backports_abc +catkin-pkg +singledispatch +virtualenv diff --git a/jsk_unitree_robot/cross/repos/ros1_dependencies.repos b/jsk_unitree_robot/cross/repos/ros1_dependencies.repos index 2983607027..a9ff0f4105 100644 --- a/jsk_unitree_robot/cross/repos/ros1_dependencies.repos +++ b/jsk_unitree_robot/cross/repos/ros1_dependencies.repos @@ -203,3 +203,9 @@ repositories: festlex-poslex: type: tar url: http://archive.ubuntu.com/ubuntu/pool/universe/f/festlex-poslex/festlex-poslex_2.4.orig.tar.gz + mailutils: + type: tar + url: http://archive.ubuntu.com/ubuntu/pool/universe/m/mailutils/mailutils_3.4.orig.tar.xz + mailutils/debian: + type: tar + url: http://archive.ubuntu.com/ubuntu/pool/universe/m/mailutils/mailutils_3.4-1.debian.tar.xz diff --git a/jsk_unitree_robot/cross/repos/unitree.repos b/jsk_unitree_robot/cross/repos/unitree.repos index 8e8e056cc1..12eb6185dc 100644 --- a/jsk_unitree_robot/cross/repos/unitree.repos +++ b/jsk_unitree_robot/cross/repos/unitree.repos @@ -21,3 +21,24 @@ repositories: type: git url: https://github.com/Affonso-Gui/app_manager version: enable-parallel-run-apps + # For parallel feature of app manager + # https://github.com/PR2/app_manager/pull/59 + google_chat_ros: + type: git + url: https://github.com/iory/jsk_3rdparty + version: google_chat_ros + # Support Google Chat API. + # https://github.com/jsk-ros-pkg/jsk_3rdparty/pull/323 + catkin_virtualenv: + type: git + url: https://github.com/iory/catkin_virtualenv + version: unitree + # At current, if we do a catkin build with CATKIN_ENABLE_TESTING=FALSE, + # we get the error 'Unknown CMake command "catkin_run_tests_target"' + # when calling catkin_generate_virtualenv. The following PR fixes this error. + # https://github.com/locusrobotics/catkin_virtualenv/pull/89 + # + # Some python environments may not have ensurepip installed. + # Also, some users may not be able to use sudo apt install to install python3-venv (sudo command), etc. + # The following PR will enable catkin_virtualenv in environments without ensurepip by doing get-pip.py within venv. + # https://github.com/locusrobotics/catkin_virtualenv/pull/90 diff --git a/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1001-urdfdom-headers b/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1001-urdfdom-headers index f9e2b9e3e0..5c43882e1c 100755 --- a/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1001-urdfdom-headers +++ b/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1001-urdfdom-headers @@ -10,4 +10,4 @@ cmake \ -DCMAKE_BUILD_TYPE=Release \ ../../src/urdfdom-headers/urdfdom_headers-1.0.0 -make -j4 install +make install diff --git a/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1002-urdfdom b/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1002-urdfdom index e4b8ea0d15..30144b316e 100755 --- a/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1002-urdfdom +++ b/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1002-urdfdom @@ -10,4 +10,4 @@ cmake \ -DCMAKE_BUILD_TYPE=Release \ ../../src/urdfdom/urdfdom-1.0.0 -make -j4 install +make install diff --git a/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1003-assimp b/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1003-assimp index 3dc23b4619..acd3d11160 100755 --- a/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1003-assimp +++ b/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1003-assimp @@ -23,4 +23,4 @@ cmake \ -DASSIMP_ENABLE_BOOST_WORKAROUND=OFF \ ${SOURCE_DIR} -make -j4 install +make install diff --git a/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1004-colladadom b/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1004-colladadom index a74c303a62..152fb4da51 100755 --- a/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1004-colladadom +++ b/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1004-colladadom @@ -27,4 +27,4 @@ cmake \ -DOPT_DOUBLE_PRECISION=ON \ ${SOURCE_DIR} -make -j4 install +make install diff --git a/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1005-yaml-cpp b/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1005-yaml-cpp index 0f1bcd80a0..1fc3ddf698 100755 --- a/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1005-yaml-cpp +++ b/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1005-yaml-cpp @@ -19,4 +19,4 @@ cmake \ -DYAML_CPP_BUILD_TOOLS=ON \ ${SOURCE_DIR} -make -j4 install +make install diff --git a/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1007-snappy b/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1007-snappy index a2a46b22b8..fb065a0a23 100755 --- a/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1007-snappy +++ b/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1007-snappy @@ -18,4 +18,4 @@ cmake \ -DBUILD_SHARED_LIBS=on \ ${SOURCE_DIR} -make -j4 install +make install diff --git a/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1008-opencv b/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1008-opencv index 4f52260bf5..adccf09285 100755 --- a/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1008-opencv +++ b/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1008-opencv @@ -50,4 +50,4 @@ cmake \ -DOPENCV_CONFIG_INSTALL_PATH=lib/cmake/OpenCV \ ${SOURCE_DIR} -make -j4 install +make install diff --git a/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1009-python-sip b/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1009-python-sip index 5920f9e9e1..dbeb077680 100755 --- a/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1009-python-sip +++ b/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1009-python-sip @@ -18,4 +18,4 @@ python \ -e /opt/jsk/${INSTALL_ROOT}/ros1_dependencies/include \ -b /opt/jsk/${INSTALL_ROOT}/ros1_dependencies/bin \ -make -j4 install +make install diff --git a/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1024-liblcm b/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1024-liblcm index 54bb3e40bc..20022e923b 100755 --- a/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1024-liblcm +++ b/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1024-liblcm @@ -20,4 +20,4 @@ automake --add-missing autoconf ./configure --prefix=/opt/jsk/${INSTALL_ROOT}/ros1_dependencies -make -j4 install +make install diff --git a/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1025-alsa-lib b/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1025-alsa-lib index 0b548baf83..32bc8248f3 100755 --- a/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1025-alsa-lib +++ b/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1025-alsa-lib @@ -16,4 +16,4 @@ cd ${SOURCE_DIR} ./configure --prefix=/opt/jsk/${INSTALL_ROOT}/ros1_dependencies -make -j4 install +make install diff --git a/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1027-jack-audio-connection-kit b/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1027-jack-audio-connection-kit index a39053b965..57e32c0585 100755 --- a/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1027-jack-audio-connection-kit +++ b/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1027-jack-audio-connection-kit @@ -17,4 +17,4 @@ cd ${SOURCE_DIR} ./autogen.sh CFLAGS="-I/opt/jsk/${INSTALL_ROOT}/ros1_dependencies/include" LIBS="-L/opt/jsk/${INSTALL_ROOT}/ros1_dependencies/lib" ./configure --enable-force-install --prefix=/opt/jsk/${INSTALL_ROOT}/ros1_dependencies -make -j4 install +make install diff --git a/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1028-libportaudio2 b/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1028-libportaudio2 index d13d19e6cd..9c6c607838 100755 --- a/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1028-libportaudio2 +++ b/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1028-libportaudio2 @@ -20,4 +20,4 @@ cmake \ -DCMAKE_BUILD_TYPE=Release \ ${SOURCE_DIR} -make -j4 install +make install diff --git a/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1030-flac b/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1030-flac index 0455aab6ba..5de19e6a74 100755 --- a/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1030-flac +++ b/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1030-flac @@ -18,4 +18,4 @@ automake --add-missing autoconf ./configure --prefix=/opt/jsk/${INSTALL_ROOT}/ros1_dependencies -make -j4 install +make install diff --git a/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1031-libncurses b/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1031-libncurses index 7638ed7532..bdcab81e05 100755 --- a/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1031-libncurses +++ b/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1031-libncurses @@ -14,4 +14,4 @@ cd ${SOURCE_DIR} ./configure --prefix=/opt/jsk/${INSTALL_ROOT}/ros1_dependencies -with-shared -make -j4 install +make install diff --git a/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1037-mailutils b/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1037-mailutils new file mode 100755 index 0000000000..510b004c88 --- /dev/null +++ b/jsk_unitree_robot/cross/ros1_dependencies_build_scripts/1037-mailutils @@ -0,0 +1,19 @@ +#!/bin/bash +set -xeu -o pipefail + +DEBIAN_DIR=/home/user/ros1_dependencies_sources/src/mailutils/debian/debian +SOURCE_DIR=/home/user/ros1_dependencies_sources/src/mailutils/mailutils-3.4 + +# +# constantly does not have patches +# +cd ${DEBIAN_DIR}/patches +for patch_file in $(grep -v ^# series); do + OUT="$(patch -p1 --forward --directory ${SOURCE_DIR} < ${patch_file} | tee /dev/tty)" || echo "${OUT}" | grep "Skipping patch" -q || (echo "$OUT" && false) || echo "OK" +done + +cd ${SOURCE_DIR} + +./configure --prefix=/opt/jsk/${INSTALL_ROOT}/ros1_dependencies + +make install diff --git a/jsk_unitree_robot/cross/run_user.sh b/jsk_unitree_robot/cross/run_user.sh index 07c9f39652..22ca920773 100755 --- a/jsk_unitree_robot/cross/run_user.sh +++ b/jsk_unitree_robot/cross/run_user.sh @@ -7,16 +7,27 @@ SOURCE_ROOT=${TARGET_MACHINE}_User set -xeuf -o pipefail +case ${OSTYPE} in + linux*) + OPTIONS="-u $(id -u $USER)" + ;; + darwin*) + OPTIONS="" + ;; +esac + # -v ${PWD}/${TARGET_MACHINE}_ws_system:/home/user/${TARGET_MACHINE}_ws_system:rw \ # run on docker docker run -it --rm \ - -u $(id -u $USER) \ + ${OPTIONS} \ -e INSTALL_ROOT=${INSTALL_ROOT} \ -v ${HOST_INSTALL_ROOT}/ros1_dependencies:/opt/jsk/${INSTALL_ROOT}/ros1_dependencies:ro \ -v ${HOST_INSTALL_ROOT}/Python:/opt/jsk/${INSTALL_ROOT}/Python:ro \ -v ${HOST_INSTALL_ROOT}/ros1_inst:/opt/jsk/${INSTALL_ROOT}/ros1_inst:ro \ -v ${HOST_INSTALL_ROOT}/ros1_dependencies_setup.bash:/opt/jsk/${INSTALL_ROOT}/ros1_dependencies_setup.bash:ro \ -v ${HOST_INSTALL_ROOT}/system_setup.bash:/opt/jsk/${INSTALL_ROOT}/system_setup.bash:ro \ + -v ${HOST_INSTALL_ROOT}/sitecustomize.py:/usr/lib/python2.7/sitecustomize.py:ro \ + -v ${HOST_INSTALL_ROOT}/sitecustomize.py:/usr/lib/python3.6/sitecustomize.py:ro \ -v ${PWD}/${SOURCE_ROOT}:/opt/jsk/User:rw \ ros1-unitree:${TARGET_MACHINE} \ bash -c "echo 'source /opt/jsk/User/user_setup.bash; env; cd /opt/jsk/User' > ~/.bashrc; exec \"\$0\"" diff --git a/jsk_unitree_robot/cross/startup_scripts/env.sh b/jsk_unitree_robot/cross/startup_scripts/env.sh new file mode 100755 index 0000000000..d158436b47 --- /dev/null +++ b/jsk_unitree_robot/cross/startup_scripts/env.sh @@ -0,0 +1,4 @@ +#!/usr/bin/env bash + +source /opt/jsk/User/user_setup.bash +exec "$@" diff --git a/jsk_unitree_robot/cross/startup_scripts/ros1_dependencies_setup.bash b/jsk_unitree_robot/cross/startup_scripts/ros1_dependencies_setup.bash index bb83142ace..2ab07f4d83 100755 --- a/jsk_unitree_robot/cross/startup_scripts/ros1_dependencies_setup.bash +++ b/jsk_unitree_robot/cross/startup_scripts/ros1_dependencies_setup.bash @@ -5,14 +5,16 @@ export CMAKE_PREFIX_PATH="/opt/jsk/System/ros1_dependencies:${CMAKE_PREFIX_PATH}" export PKG_CONFIG_PATH="/opt/jsk/System/ros1_dependencies/lib/pkgconfig:${PKG_CONFIG_PATH}" export LD_LIBRARY_PATH="/opt/jsk/System/ros1_dependencies/lib:${LD_LIBRARY_PATH}" -export PYTHONPATH="/opt/jsk/System/ros1_dependencies/lib/python2.7/site-packages:${PYTHONPATH}" +# Python's sys.path is automatically set in /usr/lib/python2.7/sitecustomize.py +# export PYTHONPATH="/opt/jsk/System/ros1_dependencies/lib/python2.7/site-packages:${PYTHONPATH}" # GI : for gir1.2-gstreamer-1.0, which is installed by ros1_dependencies_build_scripts/0006-gstreamer export GI_TYPELIB_PATH="/opt/jsk/System/ros1_dependencies/lib/girepository-1.0" # Python export LD_LIBRARY_PATH="/opt/jsk/System/Python/lib:${LD_LIBRARY_PATH}" -export PYTHONPATH="/opt/jsk/System/Python/lib/python2.7/site-packages:${PYTHONPATH}" +# Python's sys.path is automatically set in /usr/lib/python2.7/sitecustomize.py +# export PYTHONPATH="/opt/jsk/System/Python/lib/python2.7/site-packages:${PYTHONPATH}" export PATH="/opt/jsk/System/Python/bin:${PATH}" diff --git a/jsk_unitree_robot/cross/startup_scripts/sitecustomize.py b/jsk_unitree_robot/cross/startup_scripts/sitecustomize.py new file mode 100644 index 0000000000..6e0abdf861 --- /dev/null +++ b/jsk_unitree_robot/cross/startup_scripts/sitecustomize.py @@ -0,0 +1,39 @@ +# install the apport exception handler if available +import site +import sys +import os +import os.path as osp + +site.PREFIXES += [ + '/opt/jsk/System/Python/', + '/opt/jsk/System/ros1_dependencies/', + '/opt/jsk/System/ros1_inst/', +] + + +def recursively_listup_paths(path): + paths = [] + for sub_path in os.listdir(path): + sub_full_path = osp.join(path, sub_path) + if osp.isdir(sub_full_path) and (sub_path.endswith('.egg-info') or sub_path.endswith('.egg')): + paths.append(sub_full_path) + return paths + + +paths = [] +for path in site.getsitepackages(): + if osp.exists(path): + paths.append(path) + paths.extend(recursively_listup_paths(path)) + site_package = osp.join(osp.dirname(path), 'site-packages') + if site_package != path and osp.exists(site_package): + paths.append(site_package) + paths.extend(recursively_listup_paths(site_package)) + +sys.path.extend(paths) +try: + import apport_python_hook +except ImportError: + pass +else: + apport_python_hook.install() diff --git a/jsk_unitree_robot/jsk_unitree_startup/README.md b/jsk_unitree_robot/jsk_unitree_startup/README.md new file mode 100644 index 0000000000..39c1410bf4 --- /dev/null +++ b/jsk_unitree_robot/jsk_unitree_startup/README.md @@ -0,0 +1,26 @@ +# jsk_unitree_startup + +## app_manager + +The unitree launches various apps via [app_manager](https://github.com/pr2/app_manager). + +You can launch the app from the browser by accessing this [url](http://192.168.123.161:8000/rwt_app_chooser/). + +### Walk Notifier + +Send an email with the location of the walk and a camera image of the walk. + +![](./apps/walk_notifier/walk_notifier.png) + +#### prerequirement + +The key for google map needs to be placed in `/var/lib/robot/google_map_location_key.yaml` (pi@192.168.123.161) with 'location_key' as the key. + +For JSK members, the yaml file are available at [Google Drive](https://drive.google.com/file/d/1D867WB70GDEN0g9IKXfoTHk-5MUJFiuf/view?usp=sharing) + +Also, place a yaml file with the email addresses of the sender and receiver in `/var/lib/robot/walk_notifier.yaml`. + +``` +sender_address: hogehoge@some.mail.com +receiver_address: foo@some.mail.com +``` diff --git a/jsk_unitree_robot/jsk_unitree_startup/apps/emotion_reaction/emotion_reaction.app b/jsk_unitree_robot/jsk_unitree_startup/apps/emotion_reaction/emotion_reaction.app new file mode 100644 index 0000000000..2c59b16a4f --- /dev/null +++ b/jsk_unitree_robot/jsk_unitree_startup/apps/emotion_reaction/emotion_reaction.app @@ -0,0 +1,5 @@ +display: Emotion Reaction +platform: go1 +launch: jsk_unitree_startup/emotion_reaction.xml +interface: jsk_unitree_startup/emotion_reaction.interface +icon: jsk_unitree_startup/emotion_reaction.png diff --git a/jsk_unitree_robot/jsk_unitree_startup/apps/emotion_reaction/emotion_reaction.interface b/jsk_unitree_robot/jsk_unitree_startup/apps/emotion_reaction/emotion_reaction.interface new file mode 100644 index 0000000000..044105d644 --- /dev/null +++ b/jsk_unitree_robot/jsk_unitree_startup/apps/emotion_reaction/emotion_reaction.interface @@ -0,0 +1,2 @@ +published_topics: {} +subscribed_topics: {} diff --git a/jsk_unitree_robot/jsk_unitree_startup/apps/emotion_reaction/emotion_reaction.png b/jsk_unitree_robot/jsk_unitree_startup/apps/emotion_reaction/emotion_reaction.png new file mode 100644 index 0000000000..f89099f76c Binary files /dev/null and b/jsk_unitree_robot/jsk_unitree_startup/apps/emotion_reaction/emotion_reaction.png differ diff --git a/jsk_unitree_robot/jsk_unitree_startup/apps/emotion_reaction/emotion_reaction.xml b/jsk_unitree_robot/jsk_unitree_startup/apps/emotion_reaction/emotion_reaction.xml new file mode 100644 index 0000000000..0c26c32e34 --- /dev/null +++ b/jsk_unitree_robot/jsk_unitree_startup/apps/emotion_reaction/emotion_reaction.xml @@ -0,0 +1,4 @@ + + + + diff --git a/jsk_unitree_robot/jsk_unitree_startup/apps/unitree_apps.installed b/jsk_unitree_robot/jsk_unitree_startup/apps/unitree_apps.installed index 6bafcd68ef..22452458da 100644 --- a/jsk_unitree_robot/jsk_unitree_startup/apps/unitree_apps.installed +++ b/jsk_unitree_robot/jsk_unitree_startup/apps/unitree_apps.installed @@ -3,3 +3,7 @@ apps: display: go1 watch dog - app: jsk_unitree_startup/lead_teleop display: go1 lead teleop + - app: jsk_unitree_startup/walk_notifier + display: go1 walk notifier + - app: jsk_unitree_startup/emotion_reaction + display: go1 emotion reaction diff --git a/jsk_unitree_robot/jsk_unitree_startup/apps/walk_notifier/walk_notifier.app b/jsk_unitree_robot/jsk_unitree_startup/apps/walk_notifier/walk_notifier.app new file mode 100644 index 0000000000..41bc3f5339 --- /dev/null +++ b/jsk_unitree_robot/jsk_unitree_startup/apps/walk_notifier/walk_notifier.app @@ -0,0 +1,5 @@ +display: Walk Notifier +platform: go1 +launch: jsk_unitree_startup/walk_notifier.xml +interface: jsk_unitree_startup/walk_notifier.interface +icon: jsk_unitree_startup/walk_notifier.png diff --git a/jsk_unitree_robot/jsk_unitree_startup/apps/walk_notifier/walk_notifier.interface b/jsk_unitree_robot/jsk_unitree_startup/apps/walk_notifier/walk_notifier.interface new file mode 100644 index 0000000000..044105d644 --- /dev/null +++ b/jsk_unitree_robot/jsk_unitree_startup/apps/walk_notifier/walk_notifier.interface @@ -0,0 +1,2 @@ +published_topics: {} +subscribed_topics: {} diff --git a/jsk_unitree_robot/jsk_unitree_startup/apps/walk_notifier/walk_notifier.png b/jsk_unitree_robot/jsk_unitree_startup/apps/walk_notifier/walk_notifier.png new file mode 100644 index 0000000000..702466a6fc Binary files /dev/null and b/jsk_unitree_robot/jsk_unitree_startup/apps/walk_notifier/walk_notifier.png differ diff --git a/jsk_unitree_robot/jsk_unitree_startup/apps/walk_notifier/walk_notifier.xml b/jsk_unitree_robot/jsk_unitree_startup/apps/walk_notifier/walk_notifier.xml new file mode 100644 index 0000000000..8093ee49b8 --- /dev/null +++ b/jsk_unitree_robot/jsk_unitree_startup/apps/walk_notifier/walk_notifier.xml @@ -0,0 +1,6 @@ + + + + + + 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..0f993a3d75 100755 --- a/jsk_unitree_robot/jsk_unitree_startup/autostart/jsk_startup.sh +++ b/jsk_unitree_robot/jsk_unitree_startup/autostart/jsk_startup.sh @@ -17,9 +17,14 @@ if [ "$ROS_IP" == "192.168.123.161" ];then roslaunch --screen sound_play soundplay_node.launch sound_play:=robotsound & roslaunch --screen jsk_unitree_startup rwt_app_chooser.launch & roslaunch --screen jsk_unitree_startup rosserial_node.launch & + roslaunch --screen jsk_unitree_startup get_location.launch & 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 @@ -30,6 +35,7 @@ if [ "$ROS_IP" == "192.168.123.14" ];then while ! eval rostopic info /robotsound 2$toStartlog; do sleep 2; done sleep 2 # wait for a while... roslaunch jsk_unitree_startup unitree_bringup.launch network:=ethernet & + roslaunch jsk_unitree_startup google_chat_ros.launch & fi eval echo "[jsk_startup] done... " $toStartlog diff --git a/jsk_unitree_robot/jsk_unitree_startup/config/dhcpcd.conf b/jsk_unitree_robot/jsk_unitree_startup/config/dhcpcd.conf index c2d1530f29..77b0d64c07 100755 --- a/jsk_unitree_robot/jsk_unitree_startup/config/dhcpcd.conf +++ b/jsk_unitree_robot/jsk_unitree_startup/config/dhcpcd.conf @@ -76,4 +76,13 @@ nohook wpa_supplicant interface usb0 metric 100 static domain_name_servers=8.8.8.8 -############################################### \ No newline at end of file +############################################### + +################################################ +# For sparky +# This enables Unitree(sparky) to connect the Internet. +################################################ +interface wlan2 +metric 101 +static domain_name_servers=8.8.8.8 +############################################### diff --git a/jsk_unitree_robot/jsk_unitree_startup/config/iptables.ipv4.nat b/jsk_unitree_robot/jsk_unitree_startup/config/iptables.ipv4.nat new file mode 100644 index 0000000000..fdfa466fda --- /dev/null +++ b/jsk_unitree_robot/jsk_unitree_startup/config/iptables.ipv4.nat @@ -0,0 +1,38 @@ +# Generated by xtables-save v1.8.2 on Tue Apr 26 15:11:12 2022 +*filter +:INPUT ACCEPT [248560:31230459] +:FORWARD ACCEPT [107:13046] +:OUTPUT ACCEPT [979900:329384767] +-A INPUT -m state --state RELATED,ESTABLISHED -j ACCEPT +-A FORWARD -m state --state RELATED,ESTABLISHED -j ACCEPT +# Passthrough packets via LTE module. +-A FORWARD -i usb0 -o eth0 -m state --state RELATED,ESTABLISHED -j ACCEPT +-A FORWARD -o usb0 -i eth0 -m state --state RELATED,ESTABLISHED -j ACCEPT +-A FORWARD -i usb0 -o wlan1 -m state --state RELATED,ESTABLISHED -j ACCEPT +-A FORWARD -i wlan1 -o usb0 -m state --state RELATED,ESTABLISHED -j ACCEPT +# Passthrough packets via wlan2 (for sparky) +-A FORWARD -i wlan2 -o eth0 -m state --state RELATED,ESTABLISHED -j ACCEPT +-A FORWARD -o wlan2 -i eth0 -m state --state RELATED,ESTABLISHED -j ACCEPT +-A FORWARD -i wlan2 -o wlan1 -m state --state RELATED,ESTABLISHED -j ACCEPT +-A FORWARD -i wlan1 -o wlan2 -m state --state RELATED,ESTABLISHED -j ACCEPT +COMMIT +# Completed on Tue Apr 26 15:11:12 2022 +# Generated by xtables-save v1.8.2 on Tue Apr 26 15:11:12 2022 +*nat +:PREROUTING ACCEPT [276:28555] +:INPUT ACCEPT [165:13454] +:POSTROUTING ACCEPT [355:25705] +:OUTPUT ACCEPT [414:29614] +-A POSTROUTING -o wlan2 -j MASQUERADE +-A POSTROUTING -o usb0 -j MASQUERADE +COMMIT +# Completed on Tue Apr 26 15:11:12 2022 +# Generated by xtables-save v1.8.2 on Tue Apr 26 15:11:12 2022 +*mangle +:PREROUTING ACCEPT [1265282:660359078] +:INPUT ACCEPT [1262670:659548895] +:FORWARD ACCEPT [2562:800616] +:OUTPUT ACCEPT [979900:329384767] +:POSTROUTING ACCEPT [982534:330195437] +COMMIT +# Completed on Tue Apr 26 15:11:12 2022 diff --git a/jsk_unitree_robot/jsk_unitree_startup/config/sysctl.conf b/jsk_unitree_robot/jsk_unitree_startup/config/sysctl.conf new file mode 100644 index 0000000000..43006774dc --- /dev/null +++ b/jsk_unitree_robot/jsk_unitree_startup/config/sysctl.conf @@ -0,0 +1,69 @@ +# +# /etc/sysctl.conf - Configuration file for setting system variables +# See /etc/sysctl.d/ for additional system variables. +# See sysctl.conf (5) for information. +# + +#kernel.domainname = example.com + +# Uncomment the following to stop low-level messages on console +#kernel.printk = 3 4 1 3 + +##############################################################3 +# Functions previously found in netbase +# + +# Uncomment the next two lines to enable Spoof protection (reverse-path filter) +# Turn on Source Address Verification in all interfaces to +# prevent some spoofing attacks +#net.ipv4.conf.default.rp_filter=1 +#net.ipv4.conf.all.rp_filter=1 + +# Uncomment the next line to enable TCP/IP SYN cookies +# See http://lwn.net/Articles/277146/ +# Note: This may impact IPv6 TCP sessions too +#net.ipv4.tcp_syncookies=1 + +# Uncomment the next line to enable packet forwarding for IPv4 +# Required to transfer packat to jetson. +net.ipv4.ip_forward=1 + +# Uncomment the next line to enable packet forwarding for IPv6 +# Enabling this option disables Stateless Address Autoconfiguration +# based on Router Advertisements for this host +#net.ipv6.conf.all.forwarding=1 + + +################################################################### +# Additional settings - these settings can improve the network +# security of the host and prevent against some network attacks +# including spoofing attacks and man in the middle attacks through +# redirection. Some network environments, however, require that these +# settings are disabled so review and enable them as needed. +# +# Do not accept ICMP redirects (prevent MITM attacks) +#net.ipv4.conf.all.accept_redirects = 0 +#net.ipv6.conf.all.accept_redirects = 0 +# _or_ +# Accept ICMP redirects only for gateways listed in our default +# gateway list (enabled by default) +# net.ipv4.conf.all.secure_redirects = 1 +# +# Do not send ICMP redirects (we are not a router) +#net.ipv4.conf.all.send_redirects = 0 +# +# Do not accept IP source route packets (we are not a router) +#net.ipv4.conf.all.accept_source_route = 0 +#net.ipv6.conf.all.accept_source_route = 0 +# +# Log Martian Packets +#net.ipv4.conf.all.log_martians = 1 +# + +################################################################### +# Magic system request Key +# 0=disable, 1=enable all, >1 bitmask of sysrq functions +# See https://www.kernel.org/doc/html/latest/admin-guide/sysrq.html +# for what other values do +#kernel.sysrq=438 + 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/launch/emotion_reaction.launch b/jsk_unitree_robot/jsk_unitree_startup/launch/emotion_reaction.launch new file mode 100644 index 0000000000..94515aaf26 --- /dev/null +++ b/jsk_unitree_robot/jsk_unitree_startup/launch/emotion_reaction.launch @@ -0,0 +1,8 @@ + + + + + + diff --git a/jsk_unitree_robot/jsk_unitree_startup/launch/get_location.launch b/jsk_unitree_robot/jsk_unitree_startup/launch/get_location.launch new file mode 100644 index 0000000000..f77874b3c4 --- /dev/null +++ b/jsk_unitree_robot/jsk_unitree_startup/launch/get_location.launch @@ -0,0 +1,15 @@ + + + + + + + + + network_interface: $(arg network_interface) + + + + diff --git a/jsk_unitree_robot/jsk_unitree_startup/launch/google_chat_ros.launch b/jsk_unitree_robot/jsk_unitree_startup/launch/google_chat_ros.launch new file mode 100644 index 0000000000..d858a003f5 --- /dev/null +++ b/jsk_unitree_robot/jsk_unitree_startup/launch/google_chat_ros.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/jsk_unitree_robot/jsk_unitree_startup/launch/lead_teleop.launch b/jsk_unitree_robot/jsk_unitree_startup/launch/lead_teleop.launch index 975f4c99df..c4339a9d80 100644 --- a/jsk_unitree_robot/jsk_unitree_startup/launch/lead_teleop.launch +++ b/jsk_unitree_robot/jsk_unitree_startup/launch/lead_teleop.launch @@ -29,4 +29,8 @@ + + + diff --git a/jsk_unitree_robot/jsk_unitree_startup/launch/unitree_bringup.launch b/jsk_unitree_robot/jsk_unitree_startup/launch/unitree_bringup.launch index 7d06554a4d..0534bb9072 100644 --- a/jsk_unitree_robot/jsk_unitree_startup/launch/unitree_bringup.launch +++ b/jsk_unitree_robot/jsk_unitree_startup/launch/unitree_bringup.launch @@ -34,11 +34,17 @@ - + + + diff --git a/jsk_unitree_robot/jsk_unitree_startup/launch/walk_notifier.launch b/jsk_unitree_robot/jsk_unitree_startup/launch/walk_notifier.launch new file mode 100644 index 0000000000..d18374c75d --- /dev/null +++ b/jsk_unitree_robot/jsk_unitree_startup/launch/walk_notifier.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + notify_interval: $(arg notify_interval) + + + + diff --git a/jsk_unitree_robot/jsk_unitree_startup/package.xml b/jsk_unitree_robot/jsk_unitree_startup/package.xml index 9723686b9a..59e3a350d7 100644 --- a/jsk_unitree_robot/jsk_unitree_startup/package.xml +++ b/jsk_unitree_robot/jsk_unitree_startup/package.xml @@ -16,6 +16,7 @@ rosserial_python teleop_twist_joy respeaker_ros + google_chat_ros diagnostic_aggregator 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/emotion_subscriber.l b/jsk_unitree_robot/jsk_unitree_startup/scripts/emotion_subscriber.l new file mode 100755 index 0000000000..80745c6254 --- /dev/null +++ b/jsk_unitree_robot/jsk_unitree_startup/scripts/emotion_subscriber.l @@ -0,0 +1,48 @@ +#!/usr/bin/env roseus + +(ros::load-ros-manifest "roseus") + +(ros::roseus "emotion-lister") +(load "package://unitreeeus/unitree-interface.l") + +(require "package://jsk_unitree_startup/scripts/motions/happy.l") +(require "package://jsk_unitree_startup/scripts/motions/joy.l") +(require "package://jsk_unitree_startup/scripts/motions/affirmation.l") +(require "package://jsk_unitree_startup/scripts/motions/negation.l") +(require "package://jsk_unitree_startup/scripts/motions/love.l") +(require "package://jsk_unitree_startup/scripts/motions/scared.l") +(require "package://jsk_unitree_startup/scripts/motions/curious.l") +(require "package://jsk_unitree_startup/scripts/motions/astonished.l") + + +(defun emotion-cb(msg) + (let ((emotion (send msg :data))) + (ros::ros-info "Callback chatting-cb called with ~A" emotion) + (cond ((string= emotion "happy") + (happy-main)) + ((string= emotion "joy") + (joy-main)) + ((string= emotion "affirmation") + (affirmation-main)) + ((string= emotion "negation") + (negation-main)) + ((string= emotion "love") + (love-main)) + ((string= emotion "scared") + (scared-main)) + ((string= emotion "curious") + (curious-main)) + ((string= emotion "astonished") + (astonished-main)) + (t + (ros::ros-warn "called unknown emotion ~A" emotion))) + emotion)) + +(defun main() + (go1-init) + (ros::subscribe "/emotion" std_msgs::String #'emotion-cb) + (ros::rate 10) + (while (ros::ok) + (ros::spin-once))) + +(main) diff --git a/jsk_unitree_robot/jsk_unitree_startup/scripts/emotion_talker.py b/jsk_unitree_robot/jsk_unitree_startup/scripts/emotion_talker.py new file mode 100755 index 0000000000..73cc7b5422 --- /dev/null +++ b/jsk_unitree_robot/jsk_unitree_startup/scripts/emotion_talker.py @@ -0,0 +1,117 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +import rospy +from std_msgs.msg import String +from speech_recognition_msgs.msg import SpeechRecognitionCandidates +from jsk_recognition_msgs.msg import PeoplePoseArray +from dialogflow_task_executive.msg import DialogResponse +import time +import message_filters + +from threading import Lock + + +class testNode(): + def __init__(self): + # Publisher + self.pub = rospy.Publisher('/emotion', String, queue_size=1) + + # Subscriber + self.sub1 = rospy.Subscriber('/speech_to_text', SpeechRecognitionCandidates, self.speech_callback) + self.sub2 = rospy.Subscriber('/people_pose', PeoplePoseArray, callback=self.callback) + self.sub3 = rospy.Subscriber('/dialog_response', DialogResponse, callback=self.df_cb) + self.duration_time = rospy.Duration(30) + self.prev_pose_detected_time = rospy.Time.now() - self.duration_time + + self.lock = Lock() + + queue_size = 10 + fps = 100. + delay = 1 / fps * 0.5 + + def callback(self, msg): + if (rospy.Time.now() - self.prev_pose_detected_time) < self.duration_time: + return + poses = msg.poses + if len(poses) >= 1: + item = poses[0].limb_names + scores = poses[0].scores + target_limbs = [ + "left_eye", "nose", "right_eye", "right_ear", + "left_ear"] + if all([name in item for name in target_limbs]): + message = "happy" + rospy.loginfo("received {}, current emotion is {}".format(item, message)) + with self.lock: + self.publish(message) + self.prev_pose_detected_time = rospy.Time.now() + rospy.sleep(5.0) + + def publish(self, data): + self.pub.publish(data) + + def speech_callback(self, msg): + word = msg.transcript[0] + if word in ["こんにちは", + "ヤッホー", + "こんばんは", + "おはよう", + "おはようございます"]: + message = "happy" + elif word in ["可愛いね", + "可愛い", + "かわいいね", + "かわいい"]: + message = "joy" + elif word in ["散歩に行こう", + "散歩", + "行こう", + "いこう"]: + message = "affirmation" + elif word in ["今日は行けないよ", + "いけないよ", + "行けないよ"]: + message = "negation" + elif word in ["大好きだよ", + "好き", + "すき"]: + message = "love" + elif word in ["あっち行って", + "さようなら"]: + message = "scared" + else: + message = word + rospy.loginfo("received {}, current emotion is {}".format(word, message)) + with self.lock: + self.publish(message) + rospy.sleep(5.0) + + def df_cb(self, data): + if data.action == "Happy" or data.action == "input.welcome": + self.publish("happy") + elif data.action == "Smirking" or data.action == "Squinting": + self.publish("joy") + elif data.action == "Love": + self.publish("love") + elif data.action == "Fearful" or data.action == "Cry": + self.publish("scared") + elif data.action == "Relived": + self.publish("affirmation") + elif data.action == "Boring" or data.action == "Unpleasant": + self.publish("negation") + elif data.action == "input.unknown": + self.publish("curious") + elif data.action == "Angry" or data.action == "Astonished": + self.publish("astonished") + else: + rospy.logwarn("Unknown emotion") + +if __name__ == '__main__': + rospy.init_node('speech_to_emotion') + + time.sleep(3.0) + node = testNode() + + while not rospy.is_shutdown(): + rospy.sleep(0.1) diff --git a/jsk_unitree_robot/jsk_unitree_startup/scripts/human_pose_publisher.py b/jsk_unitree_robot/jsk_unitree_startup/scripts/human_pose_publisher.py index c0a130588a..930f0a907f 100755 --- a/jsk_unitree_robot/jsk_unitree_startup/scripts/human_pose_publisher.py +++ b/jsk_unitree_robot/jsk_unitree_startup/scripts/human_pose_publisher.py @@ -84,6 +84,7 @@ def on_message(client, userdata, msg): rospy.loginfo("Received `{}` from `{}` topic".format(msg.payload.decode(), msg.topic)) poses_msg = PeoplePoseArray() + poses_msg.header.stamp = self.last_received for topic in eval(msg.payload.decode()): # convert str to list by eval() pose_msg = PeoplePose() for limb, pos in topic.items(): diff --git a/jsk_unitree_robot/jsk_unitree_startup/scripts/lead-teleop-state.l b/jsk_unitree_robot/jsk_unitree_startup/scripts/lead-teleop-state.l new file mode 100755 index 0000000000..63ee24a480 --- /dev/null +++ b/jsk_unitree_robot/jsk_unitree_startup/scripts/lead-teleop-state.l @@ -0,0 +1,63 @@ +#!/usr/bin/env roseus + +(load "package://roseus_smach/src/state-machine-ros.l") + +(ros::roseus-add-msgs "jsk_recognition_msgs") + +(setq find-person-msg (instance jsk_recognition_msgs::PeoplePoseArray :init)) +(defun find-person-cb (msg) + (setq find-person-msg msg)) + +(defun start-func (args) + (let () + (set-alist 'description "Start Walking" args) + :started)) + +(defun walk-func (args) + (let ((last-time-seen-person 9999)) + (while (> last-time-seen-person 5) + (when (> (length (send find-person-msg :poses)) 0) + (setq last-time-seen-person (send (ros::time- (ros::time-now) (send find-person-msg :header :stamp)) :to-sec))) + (ros::ros-info "walk : detect person ~A sec ago" last-time-seen-person) + (ros::spin-once) + (ros::sleep)) + ;; set description and goes to next state + (set-alist 'description "Find Person" args) + :find-person)) + +(defun find-person-func (args) + (let ((last-time-seen-person 0)) + (while (< last-time-seen-person 5) + (when (> (length (send find-person-msg :poses)) 0) + (setq last-time-seen-person (send (ros::time- (ros::time-now) (send find-person-msg :header :stamp)) :to-sec))) + (ros::ros-info "person: detect person ~A sec ago" last-time-seen-person) + (ros::spin-once) + (ros::sleep)) + ; set description and goes to next state + (set-alist 'description "Start Walking" args) + :walk)) + +(defun lead-teleop-sm () + (let (sm) + (setq sm + (make-state-machine + '((:start :started :walk) + (:walk :find-person :find-person) + (:walk :finished :end) + (:find-person :walk :walk)) + '((:start 'start-func) + (:end '(lambda (&rest args) :finished)) + (:walk 'walk-func) ;; function maps + (:find-person 'find-person-func)) + '(:start) ;; initial + '(:end) ;; goal + )) + (send sm :arg-keys 'description) + sm)) + +(ros::roseus "lead-teleop-smach") +(ros::rate 1) +(ros::subscribe "/people_pose" jsk_recognition_msgs::PeoplePoseArray #'find-person-cb) +;; state machine +(exec-state-machine (lead-teleop-sm)) +(ros::exit) diff --git a/jsk_unitree_robot/jsk_unitree_startup/scripts/location_node.py b/jsk_unitree_robot/jsk_unitree_startup/scripts/location_node.py new file mode 100755 index 0000000000..62601cfc6e --- /dev/null +++ b/jsk_unitree_robot/jsk_unitree_startup/scripts/location_node.py @@ -0,0 +1,109 @@ +#!/usr/bin/env python +# -*- coding:utf-8 -*- + +import subprocess +import re + +import requests +import rospy +from std_srvs.srv import Trigger +from std_srvs.srv import TriggerResponse + + +cell_number_re = re.compile(r"^Cell\s+(?P.+)\s+-\s+Address:\s(?P.+)$") +regexps = [ + re.compile(r"^ESSID:\"(?P.*)\"$"), + re.compile(r"^Protocol:(?P.+)$"), + re.compile(r"^Mode:(?P.+)$"), + re.compile(r"^Frequency:(?P[\d.]+) (?P.+) \(Channel (?P\d+)\)$"), + re.compile(r"^Encryption key:(?P.+)$"), + re.compile(r"^Quality=(?P\d+)/(?P\d+)\s+Signal level=(?P.+) d.+$"), + re.compile(r"^Signal level=(?P\d+)/(?P\d+).*$"), +] + +# Detect encryption type +wpa_re = re.compile(r"IE:\ WPA\ Version\ 1$") +wpa2_re = re.compile(r"IE:\ IEEE\ 802\.11i/WPA2\ Version\ 1$") + + +def iwlist_scan(interface='wlan0'): + """Runs the comnmand to scan the list of networks. + + Must run as super user. + Does not specify a particular device, so will scan all network devices. + """ + cmd = ["iwlist", interface, "scan"] + proc = subprocess.Popen(cmd, stdout=subprocess.PIPE, stderr=subprocess.PIPE) + points = proc.stdout.read().decode('utf-8') + return points + + +def parse_iwlist(content): + """Parses the response from the command "iwlist scan" + + """ + cells = [] + lines = content.split('\n') + for line in lines: + line = line.strip() + cellNumber = cell_number_re.search(line) + if cellNumber is not None: + cells.append(cellNumber.groupdict()) + continue + wpa = wpa_re.search(line) + if wpa is not None: + cells[-1].update({'encryption': 'wpa'}) + wpa2 = wpa2_re.search(line) + if wpa2 is not None: + cells[-1].update({'encryption': 'wpa2'}) + for expression in regexps: + result = expression.search(line) + if result is not None: + if 'encryption' in result.groupdict(): + if result.groupdict()['encryption'] == 'on': + cells[-1].update({'encryption': 'wep'}) + else: + cells[-1].update({'encryption': 'off'}) + else: + cells[-1].update(result.groupdict()) + continue + return cells + + +class LocationNode(object): + + def __init__(self): + self.network_interface = rospy.get_param('~network_interface', 'wlan0') + self.location_key = rospy.get_param('~location_key') + self.service = rospy.Service('~get_location', + Trigger, self.get_location) + + def get_location(self, req): + aps = parse_iwlist(iwlist_scan(self.network_interface)) + contents = [{"macAddress": str(ap['mac']), "age": 0} + for ap in aps] + headers = {'Content-type': 'application/json'} + params = {'key': self.location_key, + 'language': 'ja'} + data = '{"wifiAccessPoints": ' + '[{}]'.format(contents) + '}' + + response = requests.post('https://www.googleapis.com/geolocation/v1/geolocate', + params=params, headers=headers, data=data) + # {'location': {'lat': 35.7145647, 'lng': 139.766433}, 'accuracy': 19.612} + hoge = response.json() + + lat = hoge['location']['lat'] + lng = hoge['location']['lng'] + # lat = 35.715106109567415 + # lng = 139.77380123496505 + response = requests.get( + 'https://maps.googleapis.com/maps/api/geocode/json?latlng={},{}'.format(lat, lng), + headers=headers, params=params) + return TriggerResponse(success=True, + message=response.text) + + +if __name__ == '__main__': + rospy.init_node('location_node') + location_node = LocationNode() + rospy.spin() diff --git a/jsk_unitree_robot/jsk_unitree_startup/scripts/motions/affirmation.l b/jsk_unitree_robot/jsk_unitree_startup/scripts/motions/affirmation.l new file mode 100644 index 0000000000..ac20ffd07c --- /dev/null +++ b/jsk_unitree_robot/jsk_unitree_startup/scripts/motions/affirmation.l @@ -0,0 +1,46 @@ +#!/usr/bin/env roseus \ + +(ros::load-ros-manifest "roseus") +(ros::roseus "unitree-affirmation") +;;(load "package://unitreeeus/unitree-interface.l") + +;;down +(defun affirmation-pose-1(&optional (time 3000)) + (progn (send *go1* :angle-vector (send *ri* :state :potentio-vector)) + (send *ri* :body-pose '(0 0.2 0)) + )) +;;up +(defun affirmation-pose-2(&optional (time 3000)) + (progn (send *go1* :angle-vector (send *ri* :state :potentio-vector)) + (send *ri* :body-pose '(0 -0.2 0)) + )) + +(defun reset-pose(&optional (time 3000)) + (progn (send *go1* :angle-vector (send *ri* :state :potentio-vector)) + (send *go1* :angle-vector #f(0.0 45.0 -90.0 0.0 45.0 -90.0 0.0 45.0 -90.0 0.0 45.0 -90.0)) + (send *ri* :body-pose '(0 0 0)) + )) + +(defun affirmation-main() + (print "affirmation") + ;;(go1-init) + (reset-pose 1000) + (affirmation-pose-1 800) + (unix:usleep(* 1000 400)) + (affirmation-pose-2 800) + (unix:usleep(* 1000 400)) + (affirmation-pose-1 800) + (unix:usleep(* 1000 400)) + (affirmation-pose-2 800) + (unix:usleep(* 1000 600)) + + (reset-pose 1000) + (affirmation-pose-1 800) + (unix:usleep(* 1000 400)) + (affirmation-pose-2 800) + (unix:usleep(* 1000 400)) + (affirmation-pose-1 800) + (unix:usleep(* 1000 400)) + (affirmation-pose-2 800) + (unix:usleep(* 1000 600)) + ) diff --git a/jsk_unitree_robot/jsk_unitree_startup/scripts/motions/astonished.l b/jsk_unitree_robot/jsk_unitree_startup/scripts/motions/astonished.l new file mode 100644 index 0000000000..c8d8ca80cb --- /dev/null +++ b/jsk_unitree_robot/jsk_unitree_startup/scripts/motions/astonished.l @@ -0,0 +1,41 @@ +#!/usr/bin/env roseus + +(ros::load-ros-manifest "roseus") +(ros::roseus "unitree-astonished") +;;(load "package://unitreeeus/unitree-interface.l") + +(defun astonished-pose-1(&optional (time 3000)) + (progn (send *go1* :angle-vector (send *ri* :state :potentio-vector)) + (send *go1* :body-pose (coords :pos #f(0 0 30))) + (send *ri* :body-pose (coords :pos #f(0 0 30))) + )) + +(defun astonished-pose-2(&optional (time 3000)) + (progn (send *go1* :angle-vector (send *ri* :state :potentio-vector)) + (send *go1* :body-pose '(0 -0.4 0)) + (send *ri* :body-pose '(0 -0.4 0)) + )) + +(defun reset-pose(&optional (time 3000)) + (progn (send *go1* :angle-vector (send *ri* :state :potentio-vector)) + (send *go1* :angle-vector #f(0.0 45.0 -90.0 0.0 45.0 -90.0 0.0 45.0 -90.0 0.0 45.0 -90.0)) + (send *ri* :body-pose '(0 0 0)) + )) + +(defun astonished-main() + ;;(print "astonished") + ;;(go1-init) + ;;(make-irtviewer) + ;;(send *irtviewer* :draw-objects) + ;;(objects (list *go1*)) + (reset-pose 1000) + (astonished-pose-1 800) + (unix:usleep (* 1000 1000)) + (reset-pose 800) + (unix:usleep (* 1000 400)) + (astonished-pose-2 800) + (unix:usleep (* 1000 1000)) + (reset-pose 800) + (unix:usleep (* 1000 800)) + + ) diff --git a/jsk_unitree_robot/jsk_unitree_startup/scripts/motions/curious.l b/jsk_unitree_robot/jsk_unitree_startup/scripts/motions/curious.l new file mode 100644 index 0000000000..bd76eff6a7 --- /dev/null +++ b/jsk_unitree_robot/jsk_unitree_startup/scripts/motions/curious.l @@ -0,0 +1,45 @@ +#!/usr/bin/env roseus +(ros::load-ros-manifest "roseus") +(ros::roseus "unitree-curious") +(load "package://unitreeeus/unitree-interface.l") + +(defun curious-pose-1(&optional (time 3000)) + (progn (send *go1* :angle-vector (send *ri* :state :potentio-vector)) + (send *go1* :body-pose '(0 -0.4 0)) + (send *ri* :body-pose '(0 -0.4 0)) + )) + +(defun curious-pose-2(&optional (time 3000)) + (progn (send *go1* :angle-vector (send *ri* :state :potentio-vector)) + (send *go1* :body-pose '(0.4 -0.4 0)) + (send *ri* :body-pose '(0.4 -0.4 0)) + )) + +(defun curious-pose-3(&optional (time 3000)) + (progn (send *go1* :angle-vector (send *ri* :state :potentio-vector)) + (send *go1* :body-pose '(-0.4 -0.4 0)) + (send *ri* :body-pose '(-0.4 -0.4 0)) + )) + +(defun reset-pose(&optional (time 3000)) + (progn (send *go1* :angle-vector (send *ri* :state :potentio-vector)) + (send *go1* :angle-vector #f(0.0 45.0 -90.0 0.0 45.0 -90.0 0.0 45.0 -90.0 0.0 45.0 -90.0)) + (send *ri* :body-pose '(0 0 0)) + )) + +(defun curious-main() + (print "curious") + (go1-init) + ;;(make-irtviewer) + ;;(send *irtviewer* :draw-objects) + ;;(objects (list *go1*)) + (reset-pose 1000) + (curious-pose-1 800) + (unix:usleep (* 1000 800)) + (curious-pose-2 800) + (unix:usleep (* 1000 1500)) + (curious-pose-3 800) + (unix:usleep (* 1000 1500)) + (reset-pose 800) + (unix:usleep (* 1000 800)) + ) diff --git a/jsk_unitree_robot/jsk_unitree_startup/scripts/motions/happy.l b/jsk_unitree_robot/jsk_unitree_startup/scripts/motions/happy.l new file mode 100644 index 0000000000..5dced3fdb2 --- /dev/null +++ b/jsk_unitree_robot/jsk_unitree_startup/scripts/motions/happy.l @@ -0,0 +1,48 @@ +#!/usr/bin/env roseus +(ros::load-ros-manifest "roseus") +(ros::roseus "unitree-happy") +;;(load "package://unitreeeus/unitree-interface.l") + +(defun happy-pose-1(&optional (time 3000)) + (progn (send *go1* :angle-vector (send *ri* :state :potentio-vector)) + (send *go1* :body-pose '(0.4 -0.4 -0.2)) + (send *ri* :body-pose '(0.4 -0.4 -0.2)) + )) + +(defun happy-pose-2(&optional (time 3000)) + (progn (send *go1* :angle-vector (send *ri* :state :potentio-vector)) + (send *go1* :body-pose '(-0.4 -0.4 0.2)) + (send *ri* :body-pose '(-0.4 -0.4 0.2)) + )) + +(defun reset-pose(&optional (time 3000)) + (progn (send *go1* :angle-vector (send *ri* :state :potentio-vector)) + (send *go1* :angle-vector #f(0.0 45.0 -90.0 0.0 45.0 -90.0 0.0 45.0 -90.0 0.0 45.0 -90.0)) + (send *ri* :body-pose '(0 0 0)) + )) + +(defun happy-main() + (print "happy") + ;;(go1-init) + ;;(make-irtviewer) + ;;(send *irtviewer* :draw-objects) + ;;(objects (list *go1*)) + (reset-pose 1000) + (happy-pose-1 800) + (unix:usleep (* 1000 1000)) + (reset-pose 800) + (unix:usleep (* 1000 800)) + (happy-pose-2 800) + (unix:usleep (* 1000 1000)) + (reset-pose 800) + (unix:usleep (* 1000 800)) + + (happy-pose-1 800) + (unix:usleep (* 1000 1000)) + (reset-pose 800) + (unix:usleep (* 1000 800)) + (happy-pose-2 800) + (unix:usleep (* 1000 1000)) + (reset-pose 800) + (unix:usleep (* 1000 800)) + ) diff --git a/jsk_unitree_robot/jsk_unitree_startup/scripts/motions/joy.l b/jsk_unitree_robot/jsk_unitree_startup/scripts/motions/joy.l new file mode 100644 index 0000000000..240c01d001 --- /dev/null +++ b/jsk_unitree_robot/jsk_unitree_startup/scripts/motions/joy.l @@ -0,0 +1,51 @@ +#!/usr/bin/env roseus 1;5202;0c\ + +(ros::load-ros-manifest "roseus") +(ros::roseus "unitree-joy") +;;(load "package://unitreeeus/unitree-interface.l") + +;;down +(defun joy-pose-1(&optional (time 3000)) + (progn (send *go1* :angle-vector (send *ri* :state :potentio-vector)) + (send *ri* :body-pose (coords :pos #f(0 0 -90))) + )) +;;up +(defun joy-pose-2(&optional (time 3000)) + (progn (send *go1* :angle-vector (send *ri* :state :potentio-vector)) + (send *ri* :body-pose (coords :pos #f(0 0 30))) + )) + +(defun reset-pose(&optional (time 3000)) + (progn (send *go1* :angle-vector (send *ri* :state :potentio-vector)) + (send *go1* :angle-vector #f(0.0 45.0 -90.0 0.0 45.0 -90.0 0.0 45.0 -90.0 0.0 45.0 -90.0)) + (send *ri* :body-pose '(0 0 0)) + )) + +(defun joy-main() + (print "joy") + ;;(go1-init) + (reset-pose 600) + (joy-pose-1 400) + (unix:usleep(* 1000 400)) + (joy-pose-2 800) + (unix:usleep(* 1000 400)) + (reset-pose 1000) + (unix:usleep(* 1000 400)) + (joy-pose-1 800) + (unix:usleep(* 1000 400)) + (joy-pose-2 800) + (unix:usleep(* 1000 600)) + + (reset-pose 1000) + (joy-pose-1 800) + (unix:usleep(* 1000 400)) + (joy-pose-2 800) + (unix:usleep(* 1000 400)) + (reset-pose) + (unix:usleep(* 1000 400)) + (joy-pose-1 800) + (unix:usleep(* 1000 400)) + (joy-pose-2 800) + (unix:usleep(* 1000 600)) + + ) diff --git a/jsk_unitree_robot/jsk_unitree_startup/scripts/motions/love.l b/jsk_unitree_robot/jsk_unitree_startup/scripts/motions/love.l new file mode 100644 index 0000000000..648af83202 --- /dev/null +++ b/jsk_unitree_robot/jsk_unitree_startup/scripts/motions/love.l @@ -0,0 +1,64 @@ +#!/usr/bin/env roseus \ + +(ros::load-ros-manifest "roseus") +(ros::roseus "unitree-love") +;;(load "package://unitreeeus/unitree-interface.l") + + +(defun love-pose-1(&optional (time 3000)) + (progn (send *go1* :angle-vector (send *ri* :state :potentio-vector)) + (send *ri* :body-pose '(0 0.4 -0.2)) + )) + +(defun love-pose-2(&optional (time 3000)) + (progn (send *go1* :angle-vector (send *ri* :state :potentio-vector)) + (send *ri* :body-pose '(0 0 -0.2)) + )) + +(defun love-pose-3(&optional (time 3000)) + (progn (send *go1* :angle-vector (send *ri* :state :potentio-vector)) + (send *ri* :body-pose '(0 0.4 0.2)) + )) + +(defun love-pose-4(&optional (time 3000)) + (progn (send *go1* :angle-vector (send *ri* :state :potentio-vector)) + (send *ri* :body-pose '(0 0 0.2)) + )) + +(defun reset-pose(&optional (time 3000)) + (progn (send *go1* :angle-vector (send *ri* :state :potentio-vector)) + (send *go1* :angle-vector #f(0.0 45.0 -90.0 0.0 45.0 -90.0 0.0 45.0 -90.0 0.0 45.0 -90.0)) + (send *ri* :body-pose '(0 0 0)) + )) + +(defun love-main() + (print "love") + ;;(go1-init) + (reset-pose 1000) + (unix:usleep(* 1000 600)) + (love-pose-1 800) + (unix:usleep(* 1000 400)) + (love-pose-2 800) + (unix:usleep(* 1000 400)) + (reset-pose) + (unix:usleep(* 1000 600)) + (love-pose-1 800) + (unix:usleep(* 1000 400)) + (love-pose-2 800) + (unix:usleep(* 1000 400)) + (reset-pose) + (unix:usleep(* 1000 800)) + + (love-pose-3 800) + (unix:usleep(* 1000 400)) + (love-pose-4 800) + (unix:usleep(* 1000 400)) + (reset-pose) + (unix:usleep(* 1000 600)) + (love-pose-3 800) + (unix:usleep(* 1000 400)) + (love-pose-4 800) + (unix:usleep(* 1000 400)) + (reset-pose) + (unix:usleep(* 1000 800)) + ) diff --git a/jsk_unitree_robot/jsk_unitree_startup/scripts/motions/negation.l b/jsk_unitree_robot/jsk_unitree_startup/scripts/motions/negation.l new file mode 100644 index 0000000000..88734a206e --- /dev/null +++ b/jsk_unitree_robot/jsk_unitree_startup/scripts/motions/negation.l @@ -0,0 +1,61 @@ +#!/usr/bin/env roseus \ + +(ros::load-ros-manifest "roseus") +(ros::roseus "unitree-negation") +;;(load "package://unitreeeus/unitree-interface.l") + +;;down +(defun negation-pose-1(&optional (time 3000)) + (progn (send *go1* :angle-vector (send *ri* :state :potentio-vector)) + (send *ri* :body-pose '(0 0 -0.2)) + )) +;;up +(defun negation-pose-2(&optional (time 3000)) + (progn (send *go1* :angle-vector (send *ri* :state :potentio-vector)) + (send *ri* :body-pose '(0 0 0.2)) + )) + +(defun reset-pose(&optional (time 3000)) + (progn (send *go1* :angle-vector (send *ri* :state :potentio-vector)) + (send *go1* :angle-vector #f(0.0 45.0 -90.0 0.0 45.0 -90.0 0.0 45.0 -90.0 0.0 45.0 -90.0)) + (send *ri* :body-pose '(0 0 0)) + )) + +(defun negation-main() + (print "negation") + ;;(go1-init) + (reset-pose 1000) + (negation-pose-1 800) + (unix:usleep(* 1000 200)) + (reset-pose) + (unix:usleep(* 1000 200)) + (negation-pose-2 800) + (unix:usleep(* 1000 200)) + (reset-pose) + (unix:usleep(* 1000 200)) + (negation-pose-1 800) + (unix:usleep(* 1000 200)) + (reset-pose) + (unix:usleep(* 1000 200)) + (negation-pose-2 800) + (unix:usleep(* 1000 200)) + (reset-pose) + (unix:usleep(* 1000 600)) + + (reset-pose 1000) + (negation-pose-1 800) + (unix:usleep(* 1000 200)) + (reset-pose) + (unix:usleep(* 1000 200)) + (negation-pose-2 800) + (unix:usleep(* 1000 200)) + (reset-pose) + (unix:usleep(* 1000 200)) + (negation-pose-1 800) + (unix:usleep(* 1000 200)) + (reset-pose) + (unix:usleep(* 1000 200)) + (negation-pose-2 800) + (reset-pose) + (unix:usleep(* 1000 600)) + ) diff --git a/jsk_unitree_robot/jsk_unitree_startup/scripts/motions/scared.l b/jsk_unitree_robot/jsk_unitree_startup/scripts/motions/scared.l new file mode 100644 index 0000000000..3ee72bfdac --- /dev/null +++ b/jsk_unitree_robot/jsk_unitree_startup/scripts/motions/scared.l @@ -0,0 +1,34 @@ +#!/usr/bin/env roseus \ + +(ros::load-ros-manifest "roseus") +(ros::roseus "unitree-scared") +;;(load "package://unitreeeus/unitree-interface.l") + + +(defun scared-pose-1(&optional (time 3000)) + (progn (send *go1* :angle-vector (send *ri* :state :potentio-vector)) + (send *ri* :body-pose (coords :pos #f(0 0 -90))) + )) + +(defun scared-pose-2(&optional (time 3000)) + (progn (send *go1* :angle-vector (send *ri* :state :potentio-vector)) + (send *ri* :body-pose '(0 0.3 -0.2)) + )) + +(defun reset-pose(&optional (time 3000)) + (progn (send *go1* :angle-vector (send *ri* :state :potentio-vector)) + (send *go1* :angle-vector #f(0.0 45.0 -90.0 0.0 45.0 -90.0 0.0 45.0 -90.0 0.0 45.0 -90.0)) + (send *ri* :body-pose '(0 0 0)) + )) + +(defun scared-main() + (print "scared") + ;;(go1-init) + (reset-pose 1000) + (unix:usleep(* 1000 1000)) + (scared-pose-1 800) + (unix:usleep(* 1000 1000)) + (scared-pose-2 800) + (unix:usleep(* 1000 2000)) + (reset-pose) +) 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 diff --git a/jsk_unitree_robot/jsk_unitree_startup/scripts/walk_notifier.py b/jsk_unitree_robot/jsk_unitree_startup/scripts/walk_notifier.py new file mode 100755 index 0000000000..68c6c3bce0 --- /dev/null +++ b/jsk_unitree_robot/jsk_unitree_startup/scripts/walk_notifier.py @@ -0,0 +1,104 @@ +#!/usr/bin/env python +# -*- coding:utf-8 -*- + +import os +import subprocess +import time +import re +import json +import tempfile + +import PIL.Image +import sensor_msgs.msg +from std_srvs.srv import Trigger +import rospy +import cv_bridge + + +def send_mail(place, robot_name, sender_address, receiver_address, attachment=None): + """ + Send mail with mailutils + """ + mail_title = u"{}、お散歩中です。".format(robot_name) + message = u"お散歩してます。" + if place is not None: + message += u"\n今は{}を歩いているよ。".format(place) + cmd = u"mail -s '{}' -r {} {}".format( + mail_title, sender_address, receiver_address) + if attachment is not None: + cmd += ' -A {}'.format(attachment) + cmd = ['mail', '-s', mail_title.encode('utf-8'), + '-r', sender_address, + receiver_address] + if attachment is not None: + cmd += ['-A', attachment] + rospy.loginfo('Executing: {}'.format(' '.join(cmd))) + process = subprocess.Popen(cmd, + stdin=subprocess.PIPE, + universal_newlines=True) + process.communicate(message.encode('utf-8')) + process.wait() + + +class WalkNotifier(object): + + def __init__(self): + self.bridge = cv_bridge.CvBridge() + self.robot_name = rospy.get_param('~robot_name', 'unitree').strip() + self.sub = rospy.Subscriber('~input_image', + sensor_msgs.msg.Image, + callback=self.callback, + queue_size=1) + self.sender_address = rospy.get_param('~sender_address') + self.receiver_address = rospy.get_param('~receiver_address') + self.notify_interval = int(rospy.get_param('~notify_interval', 180)) + rospy.wait_for_service('~get_location') + self.wait_image() + + def callback(self, img_msg): + self.img = self.bridge.imgmsg_to_cv2(img_msg, desired_encoding='bgr8') + + def wait_image(self): + self.img = None + rate = rospy.Rate(10) + while not rospy.is_shutdown() and self.img is None: + rate.sleep() + rospy.loginfo('[WalkMail] waiting image') + + def get_place(self): + try: + response = rospy.ServiceProxy('~get_location', Trigger)() + address = json.loads(response.message) + a = address['results'][0]['formatted_address'] + print_address = " ".join(a.split(' ')[1:]) + except Exception as e: + rospy.logerr('Error: {}'.format(str(e))) + print_address = None + if self.img is not None: + _, img_path = tempfile.mkstemp(suffix='.jpg') + PIL.Image.fromarray(self.img[..., ::-1]).save(img_path) + send_mail(print_address, self.robot_name, + self.sender_address, self.receiver_address, + img_path) + os.remove(img_path) + else: + send_mail(print_address, self.robot_name, + self.sender_address, self.receiver_address, + img_path) + + def run(self): + rate = rospy.Rate(10) + while not rospy.is_shutdown(): + rate.sleep() + try: + notifier.get_place() + except Exception as e: + rospy.logerr('Error: {}'.format(str(e))) + continue + time.sleep(self.notify_interval) + + +if __name__ == '__main__': + rospy.init_node('walk_notifier') + notifier = WalkNotifier() + notifier.run()