Skip to content

Update go_interface following autoware_state_machine removal #12

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
koji-oka-fsi wants to merge 1 commit into
base: beta/2.0.0
Choose a base branch
from
Open

Update go_interface following autoware_state_machine removal #12

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
77 changes: 67 additions & 10 deletions go_interface/go_interface.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,10 +17,13 @@

import json

from go_interface_msgs.msg import ChangeLockFlg, VehicleStatus
from go_interface_msgs.msg import VehicleStatus
from autoware_state_machine_msgs.msg import VehicleButton
from autoware_state_machine_msgs.msg import StateLock
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile
from rclpy.duration import Duration
import requests
from requests.adapters import HTTPAdapter
from std_msgs.msg import String
Expand All @@ -40,8 +43,6 @@
PATCH_CONNECT_TIMEOUT = 1.0
PATCH_READ_TIMEOUT = 2.0
PATCH_MAX_RETRY = 5


class GoInterface(Node):
def __init__(self):
super().__init__("go_interface")
Expand Down Expand Up @@ -77,14 +78,19 @@ def __init__(self):
"Content-Type": "application/json",
"Authorization": "Token {}".format(self._access_token)
}
self._current_delivery_reservation_state = StateLock.STATE_OFF
self._delivery_reservation_verification_overtime = 15.0
self._delivery_reservation_verification_time = self.get_clock().now()

# QoS Setting
depth = 1
profile = QoSProfile(depth=depth)
self._vehicle_info_subcriber = self.create_subscription(
String, "/webauto/vehicle_info", self.on_vehicle_info, profile)
self._change_lock_flg_subscriber = self.create_subscription(
ChangeLockFlg, "req_change_lock_flg", self.on_change_lock_flg, profile)
self._delivery_reservation_button_subcriber = self.create_subscription(
VehicleButton, "/delivery_reservation_button", self.on_delivery_reservation_button, profile)
self._lock_state_publisher = self.create_publisher(
StateLock, "/go_interface/lock_state", profile)
self._vehicle_status_publisher = self.create_publisher(
VehicleStatus, "api_vehicle_status", profile)

Expand All @@ -99,13 +105,11 @@ def on_change_lock_flg(self, change_lock_flg):
if not self._vehicle_id:
return

lock_flg = change_lock_flg.flg

# Patch lock-flg from server via REST API
url = "{}/api/vehicle_status".format(self._service_url)
payload = {
STR_VEHICLE_ID: self._vehicle_id,
STR_LOCK_FLG: int(lock_flg)}
STR_LOCK_FLG: int(change_lock_flg)}

try:
session = self.retry_session(retries=self._patch_max_retry)
Expand Down Expand Up @@ -144,7 +148,6 @@ def on_change_lock_flg(self, change_lock_flg):

self.fetch_from_ondemand_delivery_apps()


def on_vehicle_info(self, vehicle_info):
logger = self.get_logger()
# Parse data into json format
Expand All @@ -159,6 +162,32 @@ def on_vehicle_info(self, vehicle_info):
self._vehicle_id = vehicle_id
self._is_emergency = False

def on_delivery_reservation_button(self, delivery_reservation_info):
logger = self.get_logger()
if (self._active_schedule_exists):
logger.error(
"[go_interface] Active schedule exists.")
return
if (self._current_delivery_reservation_state == StateLock.STATE_VERIFICATION):
logger.error(
"[go_interface] Under Verification.")
return

change_lock = False
if (self._current_delivery_reservation_state == StateLock.STATE_OFF):
# Set the LED to blinking.
self._delivery_reservation_verification_time = self.get_clock().now()
self._current_delivery_reservation_state = StateLock.STATE_VERIFICATION
change_lock = True
lock_state = StateLock()
lock_state.stamp = self.get_clock().now().to_msg()
lock_state.state = self._current_delivery_reservation_state
self._lock_state_publisher.publish(lock_state)
else:
change_lock = False

self.on_change_lock_flg(change_lock)

def output_timer(self):
logger = self.get_logger()
if (self._is_emergency):
Expand All @@ -168,6 +197,7 @@ def output_timer(self):
logger.error("[go_interface] _vehicle_id is unset.")
return
self.fetch_from_ondemand_delivery_apps()
self.lock_state_verification_timeout()

def fetch_from_ondemand_delivery_apps(self):
logger = self.get_logger()
Expand Down Expand Up @@ -227,14 +257,41 @@ def fetch_from_ondemand_delivery_apps(self):
"[go_interface] \
Failed to parse active_schedule_exists retrieved from server.")

# Publish vehicle-status to autoware-state-machine
# Publish vehicle-status to eve node
vehicle_status = VehicleStatus()
vehicle_status.stamp = self.get_clock().now().to_msg()
vehicle_status.lock_flg = self._lock_flg
vehicle_status.voice_flg = self._voice_flg
vehicle_status.active_schedule_exists = self._active_schedule_exists
self._vehicle_status_publisher.publish(vehicle_status)

# lock state off or lock state verification
if ((self._current_delivery_reservation_state == StateLock.STATE_OFF)
or (self._current_delivery_reservation_state == StateLock.STATE_VERIFICATION)):
if (self._lock_flg == True):
self._current_delivery_reservation_state = StateLock.STATE_ON
change_lock_state = True
else:
if (self._lock_flg == False):
self._current_delivery_reservation_state = StateLock.STATE_OFF
change_lock_state = True

if (change_lock_state):
state_lock = StateLock()
state_lock.stamp = self.get_clock().now().to_msg()
state_lock.state = self._current_delivery_reservation_state
self._lock_state_publisher.publish(state_lock)

def lock_state_verification_timeout(self):
current_time = self.get_clock().now()
if (self._current_delivery_reservation_state == StateLock.STATE_VERIFICATION):
if ((current_time - self._delivery_reservation_verification_time) > Duration(seconds=self._delivery_reservation_verification_overtime)):
self._current_delivery_reservation_state = StateLock.STATE_OFF
state_lock = StateLock()
state_lock.stamp = self.get_clock().now().to_msg()
state_lock.state = self._current_delivery_reservation_state
self._lock_state_publisher.publish(state_lock)

def retry_session(self, retries, session=None, backoff_factor=0.3):
session = session or requests.Session()
retry = Retry(
Expand Down