41 lines
1.3 KiB
Python
41 lines
1.3 KiB
Python
#!/usr/bin/env python
|
|
|
|
from safety_user_plugin.enums.clutch_state import ClutchState as CS
|
|
from safety_user_plugin.enums.stop_state import StopState as SS
|
|
|
|
|
|
class RobotInfo:
|
|
def __init__(self, robot_id):
|
|
self.robot_id = robot_id
|
|
self.battery_voltage = None
|
|
self.linear_velocity = None
|
|
self.angular_velocity = None
|
|
self.state = None
|
|
self.clutch = None
|
|
self.obstacle_detected = None
|
|
|
|
def update_robot_info(self, robot_info_msg):
|
|
self.robot_id = robot_info_msg.robot_id.data
|
|
self.battery_voltage = robot_info_msg.battery_voltage.data
|
|
self.linear_velocity = [
|
|
robot_info_msg.twist.linear.x,
|
|
robot_info_msg.twist.linear.y,
|
|
robot_info_msg.twist.linear.z,
|
|
]
|
|
self.angular_velocity = [
|
|
robot_info_msg.twist.angular.x,
|
|
robot_info_msg.twist.angular.y,
|
|
robot_info_msg.twist.angular.z,
|
|
]
|
|
self.obstacle_detected = robot_info_msg.obstacle_detected.data
|
|
|
|
if robot_info_msg.state.data is True:
|
|
self.state = SS.RUNNING
|
|
else:
|
|
self.state = SS.STOPPED
|
|
|
|
if robot_info_msg.clutch.data is True:
|
|
self.clutch = CS.ENGAGED
|
|
else:
|
|
self.clutch = CS.DISENGAGED
|