#! /usr/bin/env python3

import rospy

import time         #タイマー作成時に必要となる
import actionlib    #SimpleActionServerクラスを提供　→　アクションのサーバー作成に必要と解釈
from action_lesson.msg import TimerAction, TimerGoal, TimerResult, TimerFeedback    #Timer.actionファイルから自動生成したメッセージクラスで必要なものを読み込む


def do_timer(goal):
    """新しいゴール（目標）を受け取ったときに呼び出される関数"""

    start_time = time.time()
    updates_count = 0           #更新回数を格納する変数を0で初期化

    #60秒を超えるタイマーは使ってほしくない　→　打ち切る（強制終了）
    if goal.time_to_wait.to_sec() > 60:
        """タイマーに制限を持たせるための処理"""
        result = TimerResult()  #TimerResultの型のリザルトメッセージ(result)を作成　←これを作成しないと，定義ファイル内のリザルトの部分にアクセスできない
        result.time_elapsed = rospy.Duration.from_sec(time.time() - start_time) #「現在時刻-開始時刻」をDuration(ROSで扱う期間)に変換してtime_elapsedフィールドに代入
        result.updates_sent = updates_count     #更新回数を記録
        server.set_aborted(result, "Timer aborted due to too-long wait")    #set_aborted()関数により，強制終了
        return

    #一度にsleepを使って目標時間の経過を待つのでなくwhileを使って刻むように時間を経過させることで，途中干渉を可能としている
    while (time.time() - start_time) < goal.time_to_wait.to_sec():

        if server.is_preempt_requested():
            """中断リクエストに対応する処理"""
            result = TimerResult()  #TimerResultの型のリザルトメッセージ(result)を作成　←これを作成しないと，定義ファイル内のリザルトの部分にアクセスできない
            result.time_elapsed = rospy.Duration.from_sec(time.time() - start_time) #「現在時刻-開始時刻」をDuration(ROSで扱う期間)に変換してtime_elapsedフィールドに代入
            result.updates_sent = updates_count     #更新回数を記録
            server.set_preempted(result, "Timer preempted") #set_preempted()関数により，強制終了（リクエストがあれば中断）
            return

        #状況をfeedbackとして記録
        feedback = TimerFeedback()  #TimerFeedbackの型のフィードバックメッセージ(feedback)を作成　←これを作成しないと，定義ファイル内のフィードバックの部分にアクセスできない
        feedback.time_elapsed = rospy.Duration.from_sec(time.time() - start_time)   #「現在時刻-開始時刻」をDuration(ROSで扱う期間)に変換してフィードバックのtime_elapsedフィールドに代入
        feedback.time_remaining = goal.time_to_wait - feedback.time_elapsed  #目標時間 - 経過時間で残り時間を計算し，フィードバックのtime_remainingに格納
        server.publish_feedback(feedback)   #クライアントに送る
        updates_count += 1  #更新回数を加算

        time.sleep(1.0) #1.0秒刻み（本当のタイマーとしては正確ではないが，あくまでもテスト）    

    result = TimerResult()  #TimerResultの型のリザルトメッセージ(result)を作成　←これを作成しないと，定義ファイル内のリザルトの部分にアクセスできない
    result.time_elapsed = rospy.Duration.from_sec(time.time() - start_time) #「現在時刻-開始時刻」をDuration(ROSで扱う期間)に変換してtime_elapsedフィールドに代入
    result.updates_sent = updates_count     #今回は0として，更新回数は考えないこととする．
    server.set_succeeded(result, "Timer completed successsfully")   #resultを引数として，set_succeeded()関数を呼び出すことで，SimpleActionServerにゴールしたことを伝える．


rospy.init_node('timer_action_server')  #ノードの初期化

server = actionlib.SimpleActionServer('timer', TimerAction, do_timer, False)    #'timer'という名で，TimerActionというアクションの型　do_timerを実行．サーバーの自動起動を無効にするためFalseを指定

server.start()  #サーバー生成後，明示的にサーバーを開始する

rospy.spin()    #ループに入り，到着すべきゴールが送られるのを待つ

#! /usr/bin/env python3

import rospy

import time
import actionlib #SimpleActionClientを使うためのパッケージ
from action_lesson.msg import TimerAction, TimerGoal, TimerResult, TimerFeedback    #アクションにアクセスするためのクラス


def feedback_cb(feedback):
    """feedbackのcallback関数"""
    print(f"[Feedback] Time elapsed: {feedback.time_elapsed.to_sec()}") #経過時間の表示
    print(f"[Feedback] Time remaining: {feedback.time_remaining.to_sec()}") #残り時間の表示



rospy.init_node('timer_action_client') #ノードの初期化

#actionlibパッケージにあるSimpleActionClientクラスを使う（アクション利用の最も簡単な方法）
client = actionlib.SimpleActionClient('timer', TimerAction) #サーバー名，型ともに利用するサーバーっと一致させる必要がある

client.wait_for_server()    #サーバーの応答を待つ

goal = TimerGoal()  #goalという変数にTimerGoalのインスタンスを生成（TimerGoalの値にアクセス可能となる）
goal.time_to_wait = rospy.Duration.from_sec(5.0)    #5.0秒をタイマーにセットrospy.Duration.from_secでrosでの期間単位Durationに変換
#goal.time_to_wait = rospy.Duration.from_sec(500.0) #500.0秒をタイマーにセット→60秒を超えているので強制終了される

client.send_goal(goal, feedback_cb = feedback_cb)   #clientとしてgoalをサーバーに送ると同時にfeedback_cb関数を呼び出す

#time.sleep(3.0)    #3秒待つ
#client.cancel_goal()   #中断のリクエストはclient.cancel_goal()で行う

client.wait_for_result()    #サーバーからの結果を待つ

State = client.get_state()                                  #ゴールの状態を数値で取得（PREEMPTED=2, SUCCEEDED=3, ABORTED=4）
Status = client.get_goal_status_text()                      #サーバーで結果を送る際に第2引数に指定した文字列を取得
Time_elapsed = client.get_result().time_elapsed.to_sec()    #取得した結果の経過時間を秒単位に変換してresultに格納
Updates = client.get_result().updates_sent                  #更新回数を取得

print(f"[Result] State: {State}")           #ゴールの状態を表示
print(f"[Result] Status: {Status}")         #ゴールの状況報告を表示
print(f"[Result] Time elapsed: {Time_elapsed}") #経過時間を表示
print(f"[Result] Updates sent: {Updates}")  #更新回数を表示