def lp_rcv_proc_dds(lp_queue):
print("DDS reciever process")
with rti.open_connector(
config_name="MyParticipantLibrary::ImageSubParticipant",
url="ImagesExample.xml") as connector:
input = connector.get_input("MySubscriber::ImageReader")
print("Waiting for publications...")
input.wait_for_publications(
) # wait for at least one matching publication
print("Waiting for data...")
while True:
input.wait() # wait for data on this input
input.take()
for sample in input.samples.valid_data_iter:
# You can get all the fields in a get_dictionary()
data = sample.get_dictionary()
# kep kiolvasasa
structured_pixels = np.array(data["pixels"],
dtype="uint8").reshape(
(len(data["pixels"]), 1))
decodedframe = cv2.imdecode(structured_pixels, flags=1)
print("new data from uuid : ", data["source"])
# cv2.imshow("kep",decodedframe)
# cv2.waitKey(1)
try:
lp_queue.put(decodedframe, True, 0)
except:
save_car_rec_loss()
print("dataloss")
time.sleep(0.001)
def start_publisher(host_id, service_type, config_xml, domain_participant_pub, data_writer):
global dds_logging_output_buffer
with rti.open_connector(
config_name=domain_participant_pub,
url=config_xml) as connector:
output = connector.get_output(data_writer)
while(True):
raw_data = dds_logging_output_buffer.get()
data_to_send = {
"measurement": raw_data["measurement"],
"tags": {
"nodeid": host_id,
"service_type": service_type,
"component": raw_data["component"]
},
"time": datetime.datetime.now().strftime(TIME_FORMAT),
"fields": {
"data": raw_data["data"]
}
}
output.instance.set_dictionary(data_to_send)
output.write()
def start_subscriber(host_id, config_xml, domain_participant_sub, data_reader,
logging_buffer):
global dds_streamer_input_buffer, dds_streamer_output_buffer
with rti.open_connector(config_name=domain_participant_sub,
url=config_xml) as connector:
input = connector.get_input(data_reader)
while (True):
input.wait() # wait for data on this input
input.take()
for sample in input.samples.valid_data_iter:
# You can get all the fields in a get_dictionary()
data = sample.get_dictionary()
if ((data["source"] != host_id)
and ((data["destination"] == "") or
(data["destination"] == host_id))):
try:
dds_streamer_input_buffer.put_nowait(data)
except:
dds_streamer_input_buffer.get()
dds_streamer_input_buffer.put(data)
if logging_buffer != None:
logging_buffer.put({
'measurement':
'test',
'component':
'dds_streamer',
'data':
'dds_streamer_input_buffer'
})
def discovery_connector_no_entity_names(self):
xml_path = os.path.join(os.path.dirname(os.path.realpath(__file__)),
"../xml/TestConnector.xml")
participant_profile = "MyParticipantLibrary::DiscoveryTestNoEntityName"
with rti.open_connector(participant_profile,
xml_path) as rti_connector:
yield rti_connector
def discovery_writer_only_connector(self):
xml_path = os.path.join(os.path.dirname(os.path.realpath(__file__)),
"../xml/TestConnector.xml")
participant_profile = "MyParticipantLibrary::DiscoveryTestWriterOnly"
with rti.open_connector(participant_profile,
xml_path) as rti_connector:
yield rti_connector
def output_thread():
with sem:
with rti.open_connector(
config_name="MyParticipantLibrary::MyParticipant",
url=xml_path) as connector:
assert connector is not None
the_output = connector.getOutput("MyPublisher::MyWriter")
assert the_output is not None
def requestreply_connector(self):
xml_path = os.path.join(os.path.dirname(os.path.realpath(__file__)),
"../xml/TestConnector.xml")
participant_profile = "MyParticipantLibrary::TestRequestReply"
with rti.open_connector(participant_profile,
xml_path) as rti_connector:
yield rti_connector
def input_thread():
with sem:
with rti.open_connector(
config_name="MyParticipantLibrary::MyParticipant",
url=xml_path) as connector:
assert connector is not None
the_input = connector.getInput("MySubscriber::MyReader")
assert the_input is not None
def one_use_connector(request):
"""Creates a Connector only for one test. Use this when
the test can't reuse a previously created connector"""
xml_path = os.path.join(os.path.dirname(os.path.realpath(__file__)),
"../xml/TestConnector.xml")
participant_profile = "MyParticipantLibrary::SingleUseParticipant"
with rti.open_connector(participant_profile, xml_path) as rti_connector:
yield rti_connector
def test_connector_double_deletion(self):
"""Verify CON-200, that Connector does not segfault on double deletion"""
participant_profile = "MyParticipantLibrary::ConnectorWithParticipantQos"
xml_path = os.path.join(os.path.dirname(
os.path.realpath(__file__)),
"../xml/TestConnector.xml")
with rti.open_connector(
config_name=participant_profile,
url=xml_path) as connector:
assert connector is not None
connector.close()
def start_publisher(host_id, config_xml, domain_participant_pub, data_writer):
global dds_streamer_input_buffer, dds_streamer_output_buffer
with rti.open_connector(config_name=domain_participant_pub,
url=config_xml) as connector:
output = connector.get_output(data_writer)
output.instance["source"] = str(host_id)
while (True):
data_to_send = dds_streamer_output_buffer.get()
output.instance.set_dictionary(data_to_send)
output.write()
def test_connector_creation_with_participant_qos(self):
"""
Tests that a domain_participant defined in XML alonside participant_qos
can be used to create a Connector object.
"""
participant_profile = "MyParticipantLibrary::ConnectorWithParticipantQos"
xml_path = os.path.join(os.path.dirname(
os.path.realpath(__file__)),
"../xml/TestConnector.xml")
with rti.open_connector(
config_name=participant_profile,
url=xml_path) as connector:
assert connector is not None
def test_load_multiple_files(self):
"""
Tests that it is possible to load two xml files using the url group syntax
"""
xml_path1 = os.path.join(os.path.dirname(os.path.realpath(__file__)),
"../xml/TestConnector.xml")
xml_path2 = os.path.join(os.path.dirname(os.path.realpath(__file__)),
"../xml/TestConnector2.xml")
with rti.open_connector(
config_name="MyParticipantLibrary2::MyParticipant2",
url=xml_path1 + ';' + xml_path2) as connector:
assert connector is not None
output = connector.get_output("MyPublisher2::MySquareWriter2")
assert output is not None
def start_subscriber(config_xml, domain_participant_sub, data_reader):
with rti.open_connector(config_name=domain_participant_sub,
url=config_xml) as connector:
input = connector.get_input(data_reader)
while (True):
input.wait() # wait for data on this input
input.take()
for sample in input.samples.valid_data_iter:
# You can get all the fields in a get_dictionary()
data = sample.get_dictionary()
#data.update({'database':'smartcity'})
data = {'points': [data]}
# write data to database
if DEBUG:
print(data)
client.write(data, {'db': db_name}, 204, 'json')
def start_publisher(host_id, config_xml, domain_participant_pub, data_writer,
logging_buffer):
global dds_streamer_input_buffer, dds_streamer_output_buffer
with rti.open_connector(config_name=domain_participant_pub,
url=config_xml) as connector:
output = connector.get_output(data_writer)
output.instance["source"] = str(host_id)
while (True):
data_to_send = dds_streamer_output_buffer.get()
output.instance.set_dictionary(data_to_send)
t1 = time.perf_counter()
output.write()
output.wait()
delay = time.perf_counter() - t1
if logging_buffer != None:
logging_buffer.put({
'measurement': 'net_delay',
'component': 'dds_streamer',
'data': str(delay)
})
# This code contains trade secrets of Real-Time Innovations, Inc. #
###############################################################################
from time import sleep
# Updating the system path is not required if you have pip-installed
# rticonnextdds-connector
from sys import path as sys_path
from os import path as os_path
file_path = os_path.dirname(os_path.realpath(__file__))
sys_path.append(file_path + "/../../../")
import rticonnextdds_connector as rti
with rti.open_connector(
config_name="DomainParticipantLibraryRR::ParticipantRequester",
url=file_path + "/RequestReplyQoS.xml") as connector:
# A initial sample is sent to get a Writer GUID generated by DDS.
# If we use a hard-coded writer GUID, samples could be discarded
# as a sample with the same writer GUID and sequence number may have been
# received before.
# This can happen if we restart the requester or we have several executions from the same code.
output = connector.get_output("RequesterPublisher::GUIDGetterWriter")
input = connector.get_input("RequesterSubscriber::GUIDGetterReader")
writer_guid = [0] * 16
output.write()
input.wait()
input.take()
for sample in input.samples.valid_data_iter:
parameters.MIXER_ROLL_PONDERATION) * parameters.MIXER_FACTOR
def read_data(source, inputName):
print("\nWaiting for ", inputName.upper(), " data...")
source.wait()
source.take()
for sample in source.samples.valid_data_iter:
data = sample.get_dictionary()
value = data[inputName]
print("Received ", inputName.upper(), ": ", value)
return value
with rti.open_connector(config_name="MyParticipantLibrary::PID_Mixer",
url="DDS.xml") as connector:
parser = argparse.ArgumentParser()
parser.add_argument("--simulated",
help="System mode [Default: Mode simulator]",
action="store_true",
default=False,
required=False)
args = parser.parse_args()
simulator_mode = args.simulated # if false, the system is in "real world" mode
roll_correction_input = connector.get_input("PID_Mixer_Sub::PIDRollReader")
height_correction_input = connector.get_input(
"PID_Mixer_Sub::PIDHeightReader")
pitch_correction_input = connector.get_input(
def start():
# global pub1
# global pub2
# rospy.init_node('path_tracking', anonymous=True)
# pub2 = rospy.Publisher('cross_track_error', Twist, queue_size=100)
# pub1 = rospy.Publisher('cmd_delta', Twist, queue_size=100)
# rospy.Subscriber("frenet_path", Path, callback_path)
# rospy.Subscriber("base_pose_ground_truth", Odometry, callback_feedback)
# rospy.spin()
with rti.open_connector(
config_name="MyParticipantLibrary::MyPurePursuitParticipant",
url=file_path + "/../Sensors_ego1.xml") as connector:
input = connector.get_input("pathSubscriber::pathSub")
input_odom = connector.get_input("StateSubscriber::stateReader")
output = connector.get_output("steeringPublisher::steeringPub")
output_speed = connector.get_output("SpeedPublisher::speedPub")
wait_topic = connector.get_input("simWaitSub::simWaitReader")
done_topic = connector.get_output("simDonePub::simDoneWriter")
# controls = connector.get_output("controlPublisher::controlPub")
# Read data from the input, transform it and write it into the output
print("Waiting for data...")
#Initialise
curr_steering = 0
curr_speed = 0
target_speed = 0
aggregate = 0
# nr_dist = 0
# all_vehicles = np.ones((max_no_of_vehicles,4))*10000
while True:
wait_topic.wait()
wait_topic.take()
print("in while loop")
wait_msg = []
for sample in wait_topic.samples.valid_data_iter:
data = sample.get_dictionary()
wait_msg = data
input_odom.wait() # Wait for data in the input
input_odom.take()
# target_point=
print("received odom data")
for sample in input_odom.samples.valid_data_iter:
data = sample.get_dictionary()
break
# vel_est = data['cdgSpeed_x']
# pos_est = [data['cdgPos_x'],data['cdgPos_y']]
# pos_est = np.asarray(pos_est)
input.wait()
# Wait for data in the input
input.take()
print("received path data")
for sample1 in input.samples.valid_data_iter:
data1 = sample1.get_dictionary()
data1 = data1['param']
# callback_path(data1)
break
data1 = np.array(data1)
data1 = np.reshape(data1, (-1, 7)).tolist()
# print(data1)
callback_path(data1)
vx = data['cdgSpeed_x']
vy = data['cdgSpeed_y']
vz = data['cdgSpeed_z']
curr_speed = math.sqrt(vx * vx + vy * vy + vz * vz)
print(F'vx->{vx} vy->{vy} vz->{vz} velocity of car {curr_speed}')
steer_angle, goal_idx = callback_feedback(data['cdgPos_x'],
data['cdgPos_y'],
data['cdgPos_z'],
data['cdgPos_heading'])
out = {}
out['AdditiveSteeringWheelAngle'] = steer_angle * math.pi / 180
out['AdditiveSteeringWheelAccel'] = 0
out['AdditiveSteeringWheelSpeed'] = 0
out['AdditiveSteeringWheelTorque'] = 0
out['MultiplicativeSteeringWheelAccel'] = 1
out['MultiplicativeSteeringWheelAngle'] = 0
out['MultiplicativeSteeringWheelSpeed'] = 1
out['MultiplicativeSteeringWheelTorque'] = 1
out['TimeOfUpdate'] = data['TimeOfUpdate']
print("XXXXX")
print("")
print("")
print("")
print(data['TimeOfUpdate'])
output.instance.set_dictionary(out)
output.write()
print("XXXXX")
# out_controls['speedsArray'] = # list of speeds
# out_controls['steeringArray'] = # list of yaws
# input_speed.wait() # Wait for data in the input
# input_speed.take()
# print("6")
# for sample in input_speed.samples.valid_data_iter:
# st11 = time.time()
# data = sample.get_dictionary()
target_speed = data1[goal_idx][5]
print(
f"Current Speed :{curr_speed}, Target Speed: {target_speed} ")
out_speed = {}
kp = 1
ki = 0
throtle = kp * (target_speed - curr_speed) + ki * aggregate
if curr_speed < start_speed:
throtle = start_throttle
print("Pedal : ", throtle)
aggregate = aggregate + (target_speed - curr_speed)
out_speed['AcceleratorAdditive'] = max(0, throtle)
out_speed['AcceleratorMultiplicative'] = 0
out_speed['BrakeAdditive'] = -min(0, throtle)
out_speed['BrakeMultiplicative'] = 0
out_speed['ClutchAdditive'] = 0
out_speed['ClutchMultiplicative'] = 0
out_speed['GearboxAutoMode'] = 1
out_speed['GearboxTakeOver'] = 0
out_speed['IsRatioLimit'] = 0
out_speed['MaxRatio'] = 1000
out_speed['MinRatio'] = 1
out_speed['ParkingBrakeAdditive'] = 0
out_speed['ParkingBrakeMultiplicative'] = 0
out_speed['ShiftDown'] = 0
out_speed['ShiftUp'] = 0
out_speed['WantedGear'] = 1
out_speed['TimeOfUpdate'] = data['TimeOfUpdate']
output_speed.instance.set_dictionary(out_speed)
output_speed.write()
done_topic.instance.set_dictionary(wait_msg)
done_topic.write()
print("message written")
# en = time.time()
# print(time.time())
print("XXXXX")
# print("")
# print("")
# print("")
print(out_speed['TimeOfUpdate'])
print(time.time())
from time import sleep
# Updating the system path is not required if you have pip-installed
# rticonnextdds-connector
from sys import path as sys_path
from os import path as os_path
import rticonnextdds_connector as rti
import sys, time
import navio.rcinput
import navio.util
with rti.open_connector(config_name="MyParticipantLibrary::Radio_Participant",
url="DDS.xml") as connector:
outputRadio = connector.get_output("Radio_Pub::RadioWriter")
#OBTENER DATOS DE RADIO
navio.util.check_apm()
rcin = navio.rcinput.RCInput()
rcin2 = navio.rcinput.RCInput()
print("Waiting for subscriptions...")
outputRadio.wait_for_subscriptions()
while (True):
period = rcin.read(2)
period2 = rcin2.read(1)
print(period + "+" + period2)
time.sleep(1)
#writers for topics
def start():
with rti.open_connector(
config_name="MyParticipantLibrary::MyPurePursuitParticipant",
url=file_path + "/../Sensors_ego1.xml") as connector:
input = connector.get_input("pathSubscriber::pathSub")
input_odom = connector.get_input("StateSubscriber::stateReader")
output = connector.get_output("steeringPublisher::steeringPub")
output_speed = connector.get_output("SpeedPublisher::speedPub")
wait_topic = connector.get_input("simWaitSub::simWaitReader")
done_topic = connector.get_output("simDonePub::simDoneWriter")
# controls = connector.get_output("controlPublisher::controlPub")
# Read data from the input, transform it and write it into the output
print("Waiting for data...")
# Initialise
# curr_steering = 0
curr_speed = 0
target_speed = 0
aggregate = 0
# nr_dist = 0
# all_vehicles = np.ones((max_no_of_vehicles,4))*10000
while True:
print("waiting")
wait_topic.wait()
wait_topic.take()
print("in while loop")
wait_msg = []
for sample in wait_topic.samples.valid_data_iter:
data = sample.get_dictionary()
wait_msg = data
input_odom.wait() # Wait for data in the input
input_odom.take()
# target_point=
print("received odom data")
for sample in input_odom.samples.valid_data_iter:
data = sample.get_dictionary()
break
# vel_est = data['cdgSpeed_x']
# pos_est = [data['cdgPos_x'],data['cdgPos_y']]
# pos_est = np.asarray(pos_est)
input.wait()
# Wait for data in the input
input.take()
print("received path data")
for sample1 in input.samples.valid_data_iter:
data1 = sample1.get_dictionary()
data1 = data1['param']
# callback_path(data1)
break
data1 = np.array(data1)
data1 = np.reshape(data1, (-1, 7)).tolist()
# print(data1.shape)
# print("----data1 starts----")
# print(data1)
# print("----data1 ends----")
callback_path(data1)
vx = data['cdgSpeed_x']
vy = data['cdgSpeed_y']
vz = data['cdgSpeed_z']
curr_speed = math.sqrt(vx * vx + vy * vy + vz * vz)
print(F'vx->{vx} vy->{vy} vz->{vz} velocity of car {curr_speed}')
steer_angle, goal_idx = callback_feedback(data['cdgPos_x'],
data['cdgPos_y'],
data['cdgPos_z'],
data['cdgPos_heading'])
out = {}
out['AdditiveSteeringWheelAngle'] = steer_angle * math.pi / 180
out['AdditiveSteeringWheelAccel'] = 0
out['AdditiveSteeringWheelSpeed'] = 0
out['AdditiveSteeringWheelTorque'] = 0
out['MultiplicativeSteeringWheelAccel'] = 1
out['MultiplicativeSteeringWheelAngle'] = 0
out['MultiplicativeSteeringWheelSpeed'] = 1
out['MultiplicativeSteeringWheelTorque'] = 1
out['TimeOfUpdate'] = data['TimeOfUpdate']
print(data['TimeOfUpdate'])
output.instance.set_dictionary(out)
output.write()
print("XXXXX")
# out_controls['speedsArray'] = # list of speeds
# out_controls['steeringArray'] = # list of yaws
# input_speed.wait() # Wait for data in the input
# input_speed.take()
# print("6")
# for sample in input_speed.samples.valid_data_iter:
# st11 = time.time()
# data = sample.get_dictionary()
###############################
target_speed = data1[goal_idx][5] #-> get from data1
###############################
# print("Current Speed : ", curr_speed)
out_speed = {}
kp1 = 1
ki1 = 0
throtle = kp1 * (target_speed - curr_speed) + ki1 * aggregate
global start_throttle
global start_speed
if curr_speed < start_speed:
throtle = start_throttle
print("Pedal : ", throtle)
aggregate = aggregate + (target_speed - curr_speed)
########################################PID MIT VEL CONTROL##########################################
tar_vel = target_speed
# tar_delta = steer_angle
active_vel = curr_speed
# plot = Twist()
# output = Twist()
output_linear_x = float()
error = tar_vel - active_vel
global error_sum
global prev_error
error_sum += error
error_diff = error - prev_error
prev_error = error
if error == 0:
if tar_vel == 0:
output_linear_x = 0
else:
output_linear_x = output_linear_x - 5 ############ why?????????????
# # updating kp, ki, kd using MIT rule
global kp
global ki
global kd
global yp
global yi
global yd
kp = kp + yp * error * error
ki = ki + yi * error * error_sum
kd = kd + yd * error * error_diff
print(f"kp is : {kp}")
print(f"ki is : {ki}")
print(f"kd is : {kd}")
global brake_threshold
# # PID on velocity with updated parameters
if error > 0.01:
output_linear_x = (kp * error + ki * error_sum +
kd * error_diff)
if error < -0.01:
output_linear_x = (
(kp * error + ki * error_sum + kd * error_diff) -
brake_threshold)
# thresholding the forward velocity
if output_linear_x > 100:
output_linear_x = 100
if output_linear_x < -100:
output_linear_x = -100
# thresholding the angle
# output_angular_z = min(30.0, max(-30.0, tar_delta))
print(f"linear velocity : {active_vel}")
print(f"target linear velocity : {tar_vel}")
print(f"delta : {tar_vel-active_vel}")
if (output_linear_x > 0):
prius_vel_throttle = output_linear_x / 100
prius_vel_brake = 0
print("acc")
print(prius_vel_throttle)
if (output_linear_x < 0):
prius_vel_brake = -output_linear_x / 100
prius_vel_throttle = 0
print("brake")
print(prius_vel_brake)
# prius_vel_steer = output_angular_z / 30
#################################PID MIT ENDS####################################
out_speed['AcceleratorAdditive'] = prius_vel_throttle
out_speed['AcceleratorMultiplicative'] = 0
out_speed['BrakeAdditive'] = prius_vel_brake
out_speed['BrakeMultiplicative'] = 0
out_speed['ClutchAdditive'] = 0
out_speed['ClutchMultiplicative'] = 0
out_speed['GearboxAutoMode'] = 1
out_speed['GearboxTakeOver'] = 0
out_speed['IsRatioLimit'] = 0
out_speed['MaxRatio'] = 1000
out_speed['MinRatio'] = 1
out_speed['ParkingBrakeAdditive'] = 0
out_speed['ParkingBrakeMultiplicative'] = 0
out_speed['ShiftDown'] = 0
out_speed['ShiftUp'] = 0
out_speed['WantedGear'] = 1
out_speed['TimeOfUpdate'] = data['TimeOfUpdate']
output_speed.instance.set_dictionary(out_speed)
output_speed.write()
done_topic.instance.set_dictionary(wait_msg)
done_topic.write()
print("message written")
print("XXXXX")
print(out_speed['TimeOfUpdate'])
print(time.time())
from time import sleep
import random
import rticonnextdds_connector as rti
with rti.open_connector(
config_name="MyParticipantLibrary::CommunicationModule",
url="../DDS.xml") as connector:
# writers for topics
outputHeight = connector.get_output(
"CommunicationModule_Pub::CommunicationModuleWriterHeight")
outputPitch = connector.get_output(
"CommunicationModule_Pub::CommunicationModuleWriterPitch")
outputRoll = connector.get_output(
"CommunicationModule_Pub::CommunicationModuleWriterRoll")
outputGps = connector.get_output(
"CommunicationModule_Pub::CommunicationModuleWriterGPS")
print("Waiting for subscriptions")
outputHeight.wait_for_subscriptions()
outputPitch.wait_for_subscriptions()
outputRoll.wait_for_subscriptions()
outputGps.wait_for_subscriptions()
speed = 0
while True:
print("Writting...")
height = random.uniform(-1, 1) # 35 to 45 cm
pitch = random.uniform(-0.05, 0.05) # -35º to 35º
roll = random.uniform(0.490865, 0.610865) # -35º to 35º
# must display this notice unaltered. #
# This code contains trade secrets of Real-Time Innovations, Inc. #
###############################################################################
from __future__ import print_function
# Updating the system path is not required if you have pip-installed
# rticonnextdds-connector
from sys import path as sys_path
from os import path as os_path
# file_path = os_path.dirname(os_path.realpath(__file__))
sys_path.append("/Users/rajive/Code/rticonnextdds-connector-py")
import rticonnextdds_connector as rti
with rti.open_connector(config_name="MyServiceIfLib::Sub",
url="if/MyService.xml") as connector:
input = connector.get_input("_sub::Chat_reader")
print("Waiting for publications...")
input.wait_for_publications() # wait for at least one matching publication
print("Waiting for data...")
for i in range(1, 500):
input.wait() # wait for data on this input
input.take()
for sample in input.samples.valid_data_iter:
# You can get all the fields in a get_dictionary()
data = sample.get_dictionary()
id = data['id']
content = data['content']
from os import path as os_path
import matplotlib.pyplot as plot
import matplotlib
import numpy
from time import sleep
# Updating the system path is not required if you have pip-installed
# rticonnextdds-connector
file_path = os_path.dirname(os_path.realpath(__file__))
sys_path.append(file_path + "/../../../")
import rticonnextdds_connector as rti
matplotlib.use('Agg') # Non-GUI backend
with rti.open_connector(
config_name = "MyParticipantLibrary::ImageSubParticipant",
url = file_path + "/ImagesExample.xml") as connector:
# Create a blank image
fig = plot.figure()
blank_image = plot.imshow(numpy.zeros((40, 60, 3)))
input = connector.get_input("MySubscriber::ImageReader")
while True:
input.read()
for sample in input.samples.valid_data_iter:
# Get the received pixels (a linear sequence)
raw_pixels = numpy.array(sample["pixels"])
# Convert the linear sequence of pixels into an Height x Width x 3
def open_test_connector(config_name):
xml_path = os.path.join(
os.path.dirname(os.path.realpath(__file__)),
"../xml/TestConnector.xml")
return rti.open_connector(config_name, xml_path)
current_control = [speed, steering]
p = current_pose + curve + current_control + nearest_vehicle
so = solver(x0=x0, p=p, lbx=lbx, ubx=ubx)
x = so['x']
u = reshape(x.T, 2, N).T
st = current_pose
con = u[0, :].T
f_value = f(st, con)
st = st + (T * f_value)
ctrlmsg = con[1]
speed_output = con[0]
return ctrlmsg, speed_output
with rti.open_connector(
config_name="MyParticipantLibrary::ObstacleParticipant",
url=file_path + "/../Sensors_ego1.xml") as connector:
input = connector.get_input("roadSubscriber::roadReader")
output = connector.get_output("steeringPublisher::steeringPub")
input_speed = connector.get_input("StateSubscriber::stateReader")
output_speed = connector.get_output("SpeedPublisher::speedPub")
input_radar_F = connector.get_input("radarSubscriber_F::radarReader_F")
input_radar_left = connector.get_input(
"radarSubscriber_left::radarReader_left")
input_radar_right = connector.get_input(
"radarSubscriber_right::radarReader_right")
# Read data from the input, transform it and write it into the output
print("Waiting for data...")
def main(args: Optional[List[str]] = None):
env: Env = Env()
info: Callable[[str], None] = _logger.info
env.info = info
EGO: int = 1
if args is not None and len(args) >= 2:
EGO = int(args[1])
print(f"Using ego {EGO}")
if args is not None and len(args) >= 3 and args[2].strip() == '--no-plot':
env.plot_paths = False
info("Starting up...")
env.global_path_handler.load_from_csv(GLOBAL_PATH_CSV_FILE)
np_path = env.global_path_handler.global_path.to_numpy()[:, :2] # Take only (x, y) from path.
env.path = np.vstack([np_path] * 6 + [np_path[:20, :]]) # Repeat for 6 laps
info(f"Loaded path from {GLOBAL_PATH_CSV_FILE} of length {len(env.path)}")
try:
with rti.open_connector(config_name="SCADE_DS_Controller::Controller",
url=get_xml_url(EGO)) as connector:
info('Opened RTI Connector')
# Readers
class Inputs:
sim_wait: Input = connector.get_input("simWaitSub::simWaitReader")
vehicle_state: Input = connector.get_input("vehicleStateOutSub::vehicleStateOutReader")
track_polys: Input = connector.get_input("camRoadLinesF1Sub::camRoadLinesF1Reader")
other_vehicle_states: Input = connector.get_input("radarFSub::radarFReader")
def list(self) -> Iterable[Input]:
return [self.sim_wait, self.vehicle_state, self.track_polys, self.other_vehicle_states]
inputs = Inputs()
# Writers
vehicle_correct = connector.getOutput("toVehicleModelCorrectivePub::toVehicleModelCorrectiveWriter")
vehicle_steer = connector.getOutput("toVehicleSteeringPub::toVehicleSteeringWriter")
sim_done = connector.getOutput("toSimDonePub::toSimDoneWriter")
sim_done.write()
controller = Controller()
while True:
load_rti(env, inputs)
# info('Got RTI inputs')
t_start = time.time()
# Remove passed points
# Note fails if robot and path have very different orientations, check for that
update_global_path(env)
use_global = False # for plotting only
# ~ 3300 ms
trajectory = run(env)
if trajectory is None:
info("Warning: Could not get trajectory, falling back to global path.")
trajectory = env.path[:31, :], generate_velocity_profile(env, env.path[:32, :])
use_global = True
# ~ 30 ms
throttle, steer = controller.run_controller_timestep(env, trajectory)
if use_global: # Emergency braking
throttle = 0.1
t_logic = time.time()
print(f"time: {(t_logic - t_start):.3f}")
if env.plot_paths: # ~ 200ms
import matplotlib.pyplot as plt
plt.clf()
plt.title(f"EGO {EGO}")
plt.gca().set_aspect('equal', adjustable='box')
plt.xlim((env.state[0] - 75, env.state[0] + 75))
plt.ylim((env.state[1] - 75, env.state[1] + 75))
from iac_planner.collision_check import CollisionChecker
cc = CollisionChecker(env, 20, time_step=0.5)
cc.init_other_paths(trajectory[0])
if len(cc.obstacles) != 0:
plt.scatter(*zip(*cc.obstacles), label='obstacles', s=5)
plt.scatter(*env.path[:40].T, label='global path', s=5)
if trajectory is not None:
plt.scatter(*trajectory[0].T, label=('local path' if not use_global else 'fake local'), s=5)
plt.arrow(env.state[0], env.state[1], 20 * np.cos(env.state[2]), 20 * np.sin(env.state[2]),
head_width=5, label='vehicle')
for i, state in enumerate(env.other_vehicle_states):
plt.arrow(state[0], state[1],
20 * np.cos(state[2]), 20 * np.sin(state[2]),
head_width=5,
label=f"other {i + 1}", color='red')
for i, path in enumerate(cc._other_vehicle_paths):
plt.scatter(*path[:2], s=5, label=f"path {i + 1}", color='red')
plt.legend()
plt.pause(0.005)
vehicle_steer.instance.setNumber("AdditiveSteeringWheelAngle", steer)
vehicle_correct.instance.setNumber("AcceleratorAdditive", throttle)
vehicle_correct.write()
vehicle_steer.write()
sim_done.write()
info("=" * 20)
except KeyboardInterrupt:
info("Keyboard interrupt")
from sys import path as sys_path
from os import path as os_path
file_path = os_path.dirname(os_path.realpath(__file__))
sys_path.append(file_path + "/../../../")
import rticonnextdds_connector as rti
import numpy as np
import math
curr_index = 0
last_index = -1
kp = 1
curr_steerings = np.zeros(20)
curr_speeds = np.zeros(20)
with rti.open_connector(config_name="MyParticipantLibrary::SpeedParticipant",
url=file_path + "/../ShapeExample.xml") as connector:
input = connector.get_input("StateSubscriber::stateReader")
output = connector.get_output("SpeedPublisher::speedPub")
input_controls = connector.get_input("controlSubscriber::controlSub")
output_steering = connector.get_output("steeringPublisher::steeringPub")
# Read data from the input, transform it and write it into the output
print("Waiting for data...")
input_controls.wait()
input.wait()
while True:
input_controls.take()
for sample in input_controls.samples.valid_data_iter:
data = sample.get_dictionary()
#print(data)
curr_steerings = data['steeringArray']
# This code contains trade secrets of Real-Time Innovations, Inc. #
###############################################################################
from time import sleep
# Updating the system path is not required if you have pip-installed
# rticonnextdds-connector
from sys import path as sys_path
from os import path as os_path
file_path = os_path.dirname(os_path.realpath(__file__))
sys_path.append(file_path + "/../")
import rticonnextdds_connector as rti
with rti.open_connector(
config_name="NewParticipantLibrary1::ShapesParticipant1",
url=file_path + "/../system_designer_shapes.xml") as connector:
output = connector.get_output("TrianglePublisher1::TriangleWriter")
print("Waiting for subscriptions...")
output.wait_for_subscriptions()
print("Writing...")
for i in range(1, 100):
output.instance.set_number("x", i)
output.instance.set_number("y", i*2)
output.instance.set_number("shapesize", 30)
output.instance.set_string("color", "BLUE")
output.write()
