#!/usr/bin/env python
import rospy
from std_msgs.msg import String
from periphery import I2C
from distance_sensor.msg import sensor_values
import time
import smbus
# Globals.
data = sensor_values()
pub = rospy.Publisher('sensor_distance',
data_class=sensor_values,
queue_size=1000)
rospy.init_node('Teraranger', anonymous=False)
update_rate = rospy.Rate(5)
DEVICE_ADDRESS_LEFT = 0x31
DEVICE_REG_MODE_TRIGGER_LEFT = 0x62
DEVICE_REG_MODE_READ_LEFT = 0x63
DEVICE_ADDRESS_MIDDLE = 0x33
DEVICE_REG_MODE_TRIGGER_MIDDLE = 0x66
DEVICE_REG_MODE_READ_MIDDLE = 0x67
DEVICE_ADDRESS_RIGHT = 0x35
DEVICE_REG_MODE_TRIGGER_RIGHT = 0x6a
DEVICE_REG_MODE_READ_RIGHT = 0x6b
bus = smbus.SMBus(0)
# topics:
publish_from_tx2_to_rio_name = 'control_to_rio'
subscibe_from_rio_to_tx2_name = 'read_rio'
subscribe_vehicle_settings = 'vehicle_settings'
subscribe_canny_name = 'errorDistances'
#publish_emstop_name = 'emergency_stop_internal'
publish_emstop_name = 'emergency_stop_external'
# Globals.
message_to_rio = to_rio()
message_from_rio = from_rio()
message_settings = vehicle_settings()
message_canny = errorDistances()
message_stop = stop()
message_from_terra = sensor_values()
# Create a publisher node so the rio can subsribe on the destinations.
tx2_to_rio_pub_handle = rospy.Publisher(publish_from_tx2_to_rio_name,
data_class=to_rio,
queue_size=1)
tx2_emstop_pub_handle = rospy.Publisher(publish_emstop_name,
data_class=stop,
queue_size=1)
#tx2_to_ext_pub_handle = rospy.Publisher(publish_vehicle_status, data_class=vehicle_status, queue_size=1)
# Create the nodes in the car.
rospy.init_node('tx2_main', anonymous=False)
update_rate = rospy.Rate(100) #100hz
from ros_labview_dummy.msg import from_rio
#from ros_labview.msg import to_rio
from ros_labview.msg import vehicle_status
from distance_sensor.msg import sensor_values
# Settings:
#publish_from_tx2_to_rio_name = 'control_to_rio'
publish_status_name = 'vehicle_status'
subscibe_from_rio_to_tx2_name = 'read_rio'
subscrib_to_distance_name = 'sensor_distance'
# Globals.
#message_to_rio = to_rio()
message_from_rio = from_rio()
message_status = vehicle_status()
front_sensors = sensor_values()
# Create a publisher node so the rio can subsribe on the destinations.
#pub_handle = rospy.Publisher(publish_from_tx2_to_rio_name, data_class=to_rio, queue_size=1)
pub_handle = rospy.Publisher(publish_status_name,
data_class=vehicle_status,
queue_size=1)
rospy.init_node('tx2_to_controll.py', anonymous=True)
update_rate = rospy.Rate(10) #100hz
# Subscribe to the data from rio
status = 0 # This is a vehicle_status struct
front_dist = 0 # This is a front_sensors struct
counter = 0
def send_status():
"""TODO: for send status to a eventual controll gui.