From 08f15df4823390dd2d60f6172d5e95a88a76c984 Mon Sep 17 00:00:00 2001 From: iory Date: Sun, 7 Aug 2022 19:19:41 +0900 Subject: [PATCH] [jsk_robot_startup] add launch/get_location.launch and scripts/location_node.py [jsk_unitree_startup] start get_location.launch in jsk_startup.sh, add how to set GoogleMap keys --- .../launch/get_location.launch | 15 +++ .../scripts/location_node.py | 109 ++++++++++++++++++ .../jsk_unitree_startup/README.md | 7 ++ .../autostart/jsk_startup.sh | 1 + 4 files changed, 132 insertions(+) create mode 100644 jsk_robot_common/jsk_robot_startup/launch/get_location.launch create mode 100755 jsk_robot_common/jsk_robot_startup/scripts/location_node.py create mode 100644 jsk_unitree_robot/jsk_unitree_startup/README.md diff --git a/jsk_robot_common/jsk_robot_startup/launch/get_location.launch b/jsk_robot_common/jsk_robot_startup/launch/get_location.launch new file mode 100644 index 0000000000..a7c708b52d --- /dev/null +++ b/jsk_robot_common/jsk_robot_startup/launch/get_location.launch @@ -0,0 +1,15 @@ + + + + + + + + + network_interface: $(arg network_interface) + + + + diff --git a/jsk_robot_common/jsk_robot_startup/scripts/location_node.py b/jsk_robot_common/jsk_robot_startup/scripts/location_node.py new file mode 100755 index 0000000000..62601cfc6e --- /dev/null +++ b/jsk_robot_common/jsk_robot_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/README.md b/jsk_unitree_robot/jsk_unitree_startup/README.md new file mode 100644 index 0000000000..447343b910 --- /dev/null +++ b/jsk_unitree_robot/jsk_unitree_startup/README.md @@ -0,0 +1,7 @@ +# jsk_unitree_startup + +#### 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) 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..6692672c87 100755 --- a/jsk_unitree_robot/jsk_unitree_startup/autostart/jsk_startup.sh +++ b/jsk_unitree_robot/jsk_unitree_startup/autostart/jsk_startup.sh @@ -17,6 +17,7 @@ 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_robot_startup get_location.launch & roslaunch --screen respeaker_ros sample_respeaker.launch language:=ja-JP publish_tf:=false launch_soundplay:=false & fi