クラウドの力でロボットを遠隔から操作してみよう！

import rospy
from rospy.exceptions import ROSInterruptException
from geometry_msgs.msg import Twist
from awsiot import mqtt_connection_builder
from awscrt import mqtt, http
import json
from math import pi
####追加分の始まり####
import ssl
import http.client
import subprocess
import sys
import os
import signal
####追加分の終わり####

host = '<ホスト>'
thingsName = 'turtlebot3-sim'
topic = thingsName + '/control'

certsPath = './certs/'
rootCAPath = certsPath + '<Amazon Root CA1>'
certificatePath = certsPath + '<デバイス証明書>'
privateKeyPath = certsPath + '<プライベートキーファイル>'
port = 8883

####追加分の始まり####
credentialsEndpoint = '<IoT Credentialsのエンドポイント>'
roleAlias = 'turtlebot3-kvs-role-alias'
region = '<使用されているAWS Region>'

context = ssl.SSLContext(ssl.PROTOCOL_SSLv23)
context.load_cert_chain(certificatePath, privateKeyPath)
context.load_verify_locations(rootCAPath)
context.verify_mode = ssl.CERT_REQUIRED
context.check_hostname = True

headers={
    "x-amzn-iot-thingname": thingsName
}
conn = http.client.HTTPSConnection(credentialsEndpoint, port=443, context=context)
conn.request("GET", "/role-aliases/turtlebot3-kvs-role-alias/credentials", headers=headers)
print("Requesting credentials...")
response = conn.getresponse()
if response.status != 200:
    print("status:{} reason:{}".format(response.status, response.reason))
    sys.exit(1)
if response.status == 200:
    print("Got credentials!")
    
credential_data = json.loads(response.read())

environmentVars = {
    "IOT_GET_CREDENTIAL_ENDPOINT": credentialsEndpoint, 
    "ROLE_ALIAS": roleAlias, 
    "AWS_DEFAULT_REGION": region, 
    "DEFAULT_KVS_CACERT_PATH": rootCAPath, 
    "AWS_ACCESS_KEY_ID": credential_data['credentials']['accessKeyId'], 
    "AWS_SECRET_ACCESS_KEY": credential_data['credentials']['secretAccessKey'], 
    "AWS_SESSION_TOKEN": credential_data['credentials']['sessionToken']
}
    
command = ['./kvsWebrtcClientMasterGstSample', thingsName]
proc = subprocess.Popen(command, stdout=subprocess.PIPE, env=environmentVars)
####追加分の終わり####
        
def shutdown_turtlebot():
    disconnect_future = mqtt_connection.disconnect()
    disconnect_future.result()
    print("Disconnected from AWS IoT!")
    rospy.loginfo('Stop Turtlebot')
    cmd_vel_pub.publish(Twist())
    rospy.sleep(1)
####追加分の始まり####
    os.killpg(os.getpgid(proc.pid), signal.SIGTERM)
####追加分の終わり####
    
cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
rospy.init_node('turtlebot3_sim_control_node')
rospy.on_shutdown(shutdown_turtlebot)
control = Twist()
rate = rospy.Rate(1)

def handleMessage(topic, payload):
    duration = 4
    print("Received a new message: ")
    print(payload)
    print("from topic: ")
    print(topic)
    print("--------------\n\n")
    payload = json.loads(payload)
    x = 0
    z = 0
    if payload["control"] == "forward":
        x = 0.1
    if payload["control"] == "stop":
        x = 0
        z = 0
    if payload["control"] == "backward":
        x = -0.1
    if payload["control"] == "left":
        z = pi*0.8/4/duration
    if payload["control"] == "right":
        z = -(pi*0.8/4/duration)
    control.linear.x = x
    control.angular.z = z

# AWS IoT MQTT Client connection configuration
mqtt_connection = mqtt_connection_builder.mtls_from_path(
        endpoint=host,
        port=port,
        cert_filepath=certificatePath,
        pri_key_filepath=privateKeyPath,
        ca_filepath=rootCAPath,
        client_id=thingsName,
        clean_session=False,
        keep_alive_secs=30)

# Connect to AWS IoT
connect_future = mqtt_connection.connect()

# Waits until a result is available
connect_future.result()
print("Connected to AWS IoT!")

# Subscribe to AWS IoT
subscribe_future, packet_id = mqtt_connection.subscribe(
        topic=topic,
        qos=mqtt.QoS.AT_LEAST_ONCE,
        callback=handleMessage)
subscribe_result = subscribe_future.result()
print("Subscribed with {}".format(str(subscribe_result['qos'])))

while not rospy.is_shutdown():
    try:
        cmd_vel_pub.publish(control)
        rate.sleep()
    except ROSInterruptException:
        rospy.loginfo('Finish')