├── pycarmaker ├── __init__.py └── CarMaker.py ├── examples ├── Ex03_Reading_camera.py ├── Ex04_DVAWrite.py ├── C │ ├── README.md │ └── APO_Ex_01_Read_Quant.c ├── Problem01_FirstTwoReads.py ├── Ex02_ReadingMultipleQuantities_Spd_Yaw_SteerAng.py ├── Ex01_ReadVehicleSpeed.py └── cm_ros_node.py ├── setup.py └── README.md /pycarmaker/__init__.py: -------------------------------------------------------------------------------- 1 | from .CarMaker import CarMaker, Quantity, VDS 2 | 3 | # if somebody does "from somepackage import *", this is what they will 4 | # be able to access: 5 | __all__ = [ 6 | 'CarMaker', 'Quantity', 'VDS' 7 | ] 8 | -------------------------------------------------------------------------------- /examples/Ex03_Reading_camera.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import os 3 | import logging 4 | from types import * 5 | import time 6 | import cv2 7 | from pycarmaker import VDS 8 | 9 | # initalize VDS 10 | vds = VDS() 11 | # Connect 12 | vds.connect() 13 | # Read Images 14 | while(True): 15 | # Capture frame-by-frame 16 | frame = vds.read() 17 | frame = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR) 18 | # Display the resulting frame 19 | cv2.imshow('Frame', frame) 20 | # Press Q on keyboard to exit 21 | if cv2.waitKey(25) & 0xFF == ord('q'): 22 | break 23 | -------------------------------------------------------------------------------- /examples/Ex04_DVAWrite.py: -------------------------------------------------------------------------------- 1 | from pycarmaker import CarMaker, Quantity 2 | import time 3 | 4 | # 1 - Initialize pyCarMaker 5 | IP_ADDRESS = "localhost" 6 | PORT = 16660 7 | cm = CarMaker(IP_ADDRESS, PORT) 8 | 9 | # 2 - Connect to CarMaker 10 | cm.connect() 11 | 12 | # 3 - Create a Quantity 13 | qbrake = Quantity("DM.Brake", Quantity.FLOAT) 14 | 15 | # 4 - Press the Brake 16 | cm.DVA_write(qbrake, 1) 17 | # cm.send("DVAReleaseQuants\r") 18 | 19 | # 5 - wait for 5 seconds 20 | time.sleep(5) 21 | 22 | # 6 - Release the Brake 23 | cm.DVA_write(qbrake, 0) 24 | 25 | # 7 - Wait for 5 seconds 26 | time.sleep(5) 27 | 28 | # 8 - Release the DVA 29 | cm.DVA_release() 30 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | import setuptools 2 | 3 | with open("README.md", "r") as fh: 4 | long_description = fh.read() 5 | 6 | setuptools.setup( 7 | name="pycarmaker", # Replace with your own username 8 | version="0.0.1", 9 | author="Gustavo Muller Nunes", 10 | author_email="gustavo.vh@gmail.com", 11 | description="CarMaker Python Interface", 12 | long_description=long_description, 13 | long_description_content_type="text/markdown", 14 | url="https://github.com/gmnvh/pycarmaker", 15 | packages=setuptools.find_packages(), 16 | classifiers=[ 17 | "Programming Language :: Python :: 3", 18 | "License :: OSI Approved :: MIT License", 19 | "Operating System :: OS Independent", 20 | ], 21 | 22 | ) 23 | -------------------------------------------------------------------------------- /examples/C/README.md: -------------------------------------------------------------------------------- 1 | # APO library 2 | 3 | The pycarmaker library is a Python class that tries to copy the functionality of the APO library in case you need to write your application in Python. 4 | But if you are writing in C, IPG already has the APO library to help you to write and read quantities. 5 | 6 | Here you will find some simple examples. IPG already has a more complex example that you could use as a baseline for you application. 7 | The idea of these examples here is to explain how APO works and better understand the IPG code. 8 | 9 | IPG example can be found in the installation folder _IPG\carmaker\win64-8.0.2\Examples\APO_. 10 | 11 | # Example 1 - Read quantities 12 | 13 | This example will connect to carmaker and read time and vehicle speed. 14 | 15 | 16 | # How to compile the code on Visual Studio 2019 17 | 18 | https://youtu.be/2NYBEu6yNTk 19 | -------------------------------------------------------------------------------- /examples/Problem01_FirstTwoReads.py: -------------------------------------------------------------------------------- 1 | import time 2 | import socket 3 | 4 | 5 | # Computer on which the TCg / IP port is opened 6 | TCP_IP = 'localhost' 7 | # Port number 8 | TCP_PORT = 16660 9 | BUFFER_SIZE = 10240 10 | 11 | # Open the TCP / IP port in Python 12 | s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) 13 | # Connect to the CM TCP / IP port 16660 on localhost 14 | s.connect((TCP_IP, TCP_PORT)) 15 | 16 | MESSAGE = "QuantSubscribe {Time Car.v Car.tx Car.ty}\r" 17 | s.send(MESSAGE.encode()) 18 | s_string_val = s.recv(10) 19 | 20 | 21 | MSG_VEH_POS_X = "expr {$Qu(Car.tx)}\r" 22 | MSG_VEH_POS_Y = "expr {$Qu(Car.ty)}\r" 23 | MSG_VEH_SPEED = "expr {$Qu(Car.v)*3.6}\r" 24 | MSG_SIM_STATUS = "SimStatus\r" 25 | 26 | MSG = MSG_SIM_STATUS + MSG_VEH_SPEED + MSG_VEH_POS_X + MSG_VEH_POS_Y 27 | print(MSG.encode()) 28 | 29 | # Try 1 30 | s.send(MSG.encode()) 31 | str_rx = s.recv(200).decode() 32 | rx_list = str_rx.split("\r\n\r\n") 33 | print('Try 1') 34 | print(rx_list) 35 | 36 | time.sleep(0.1) 37 | 38 | # Try 2 39 | s.send(MSG.encode()) 40 | str_rx = s.recv(200).decode() 41 | rx_list = str_rx.split("\r\n\r\n") 42 | print('Try 2') 43 | print(rx_list) 44 | 45 | time.sleep(0.1) 46 | 47 | # Try 3 48 | s.send(MSG.encode()) 49 | str_rx = s.recv(200).decode() 50 | rx_list = str_rx.split("\r\n\r\n") 51 | print('Try 3') 52 | print(rx_list) 53 | 54 | time.sleep(0.1) 55 | 56 | # Try 4 57 | s.send(MSG.encode()) 58 | str_rx = s.recv(200).decode() 59 | rx_list = str_rx.split("\r\n\r\n") 60 | print('Try 4') 61 | print(rx_list) -------------------------------------------------------------------------------- /examples/Ex02_ReadingMultipleQuantities_Spd_Yaw_SteerAng.py: -------------------------------------------------------------------------------- 1 | import math 2 | import sys 3 | import os 4 | import logging 5 | from types import * 6 | import time 7 | 8 | # Include CarMaker 9 | from pycarmaker import CarMaker, Quantity 10 | 11 | # Welcome message 12 | print("Ex 02 - Reading multiple quantities - vehicle speed, yaw and steering angle\r\n") 13 | 14 | # Enable logging 15 | FORMAT = '[%(levelname)6s] %(module)10s: %(message)s' 16 | logging.basicConfig(format=FORMAT) 17 | 18 | # 1 - Open CarMaker with option -cmdport 19 | ''' 20 | For example: on a Windows system with CarMaker 8.0.2 installed on the default 21 | folder send the command C:\IPG\carmaker\win64-8.0.2\bin\CM.exe -cmdport 16660 22 | ''' 23 | 24 | # 2 - Start any TesRun 25 | 26 | # 3 - Initialize pyCarMaker 27 | IP_ADDRESS = "localhost" 28 | PORT = 16660 29 | 30 | cm = CarMaker(IP_ADDRESS, PORT) 31 | 32 | # 4 - Connect to CarMaker 33 | cm.connect() 34 | 35 | # 5 - Subscribe to quantities 36 | 37 | # Create Quantity instances for vehicle speed, yaw and steering angle 38 | vehspd = Quantity("Car.v", Quantity.FLOAT) 39 | caryaw = Quantity("Car.Yaw", Quantity.FLOAT) 40 | steerang = Quantity("Driver.Steer.Ang", Quantity.FLOAT) 41 | 42 | # Initialize with negative speed to indicate that value was not read 43 | vehspd.data = -1.0 44 | 45 | # Subscribe (TCP socket need to be connected) 46 | cm.subscribe(vehspd) 47 | cm.subscribe(caryaw) 48 | cm.subscribe(steerang) 49 | 50 | # 6 - Read all subscribed quantities. In this example, vehicle speed, yaw and steering angle. 51 | 52 | 53 | c = 10 54 | while(c > 0): 55 | c = c - 1 56 | 57 | # Read data from carmaker 58 | cm.read() 59 | 60 | print() 61 | print("Vehicle speed: " + str(vehspd.data * 3.6) + " km/h") 62 | print("Vehicle yaw: " + str(round(math.degrees(caryaw.data), 2))) 63 | print("Steering angle: " + str(round(math.degrees(steerang.data), 2))) 64 | time.sleep(1) 65 | -------------------------------------------------------------------------------- /examples/Ex01_ReadVehicleSpeed.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import os 3 | import logging 4 | from types import * 5 | import time 6 | 7 | # Include CarMaker 8 | from pycarmaker import CarMaker, Quantity 9 | 10 | # Welcome message 11 | print("Ex 01 - Reading vehicle speed\r\n") 12 | 13 | # Enable logging 14 | FORMAT = '[%(levelname)6s] %(module)10s: %(message)s' 15 | logging.basicConfig(format=FORMAT) 16 | 17 | # 1 - Open CarMaker with option -cmdport 18 | ''' 19 | For example: on a Windows system with CarMaker 8.0.2 installed on the default 20 | folder send the command C:\IPG\carmaker\win64-8.0.2\bin\CM.exe -cmdport 16660 21 | ''' 22 | 23 | # 2 - Start any TesRun 24 | 25 | # 3 - Initialize pyCarMaker 26 | IP_ADDRESS = "localhost" 27 | PORT = 16660 28 | 29 | cm = CarMaker(IP_ADDRESS, PORT) 30 | 31 | # 4 - Connect to CarMaker 32 | cm.connect() 33 | 34 | # 5 - Subscribe to vehicle speed 35 | 36 | # Create a Quantity instance for vehicle speed (vehicle speed is a float type variable) 37 | vehspd = Quantity("Car.v", Quantity.FLOAT) 38 | 39 | # Initialize with negative speed to indicate that value was not read 40 | vehspd.data = -1.0 41 | 42 | # Subscribe (TCP socket need to be connected) 43 | cm.subscribe(vehspd) 44 | 45 | # Let's also read the simulation status (simulation status is not a quantity but a command 46 | # so the command parameter must be set to True) 47 | sim_status = Quantity("SimStatus", Quantity.INT, True) 48 | cm.subscribe(sim_status) 49 | 50 | # 6 - Read all subscribed quantities. In this example, vehicle speed and simulation status 51 | 52 | # For some reason, the first two reads will be incomplete and must be ignored 53 | # You will see 2 log errors like this: [ ERROR] CarMaker: Wrong read 54 | cm.read() 55 | cm.read() 56 | time.sleep(0.1) 57 | 58 | c = 5 59 | while(c > 0): 60 | c = c - 1 61 | 62 | # Read data from carmaker 63 | cm.read() 64 | 65 | print() 66 | print("Vehicle speed: " + str(vehspd.data * 3.6) + " km/h") 67 | print("Simulation status: " + ("Running" if sim_status.data >= 68 | 0 else cm.status_dic.get(sim_status.data))) 69 | time.sleep(1) 70 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # pycarmaker 2 | 3 | Python class to control, write and read information from CarMaker (https://ipg-automotive.com/). 4 | 5 | If you are writing your application in C, use the APO library. Here is an example: https://github.com/gmnvh/pycarmaker/tree/master/examples/C 6 | 7 | ## How to install 8 | 9 | `pip install -e . ` 10 | 11 | ## How to use pycarmaker 12 | 13 | ```python 14 | from pycarmaker import CarMaker, Quantity 15 | 16 | # 1 - Open CarMaker with option -cmdport 17 | ''' 18 | For example: on a Windows system with CarMaker 8.0.2 installed on the default 19 | folder send the command C:\IPG\carmaker\win64-8.0.2\bin\CM.exe -cmdport 16660 20 | ''' 21 | 22 | # 2 - Start any TesRun 23 | 24 | # 3 - Initialize pyCarMaker 25 | IP_ADDRESS = "localhost" 26 | PORT = 16660 27 | cm = CarMaker(IP_ADDRESS, PORT) 28 | 29 | # 4 - Connect to CarMaker 30 | cm.connect() 31 | 32 | # 5 - Subscribe to vehicle speed 33 | # Create a Quantity instance for vehicle speed (vehicle speed is a float type variable) 34 | vehspd = Quantity("Car.v", Quantity.FLOAT) 35 | 36 | # Initialize with negative speed to indicate that value was not read 37 | vehspd.data = -1.0 38 | 39 | # Subscribe (TCP socket need to be connected) 40 | cm.subscribe(vehspd) 41 | 42 | # Let's also read the simulation status (simulation status is not a quantity but a command 43 | # so the command parameter must be set to True) 44 | sim_status = Quantity("SimStatus", Quantity.INT, True) 45 | cm.subscribe(sim_status) 46 | 47 | # 6 - Read all subscribed quantities. In this example, vehicle speed and simulation status 48 | # For some reason, the first two reads will be incomplete and must be ignored 49 | # You will see 2 log errors like this: [ ERROR] CarMaker: Wrong read 50 | cm.read() 51 | cm.read() 52 | time.sleep(0.1) 53 | c = 5 54 | while(c > 0): 55 | c = c - 1 56 | # Read data from carmaker 57 | cm.read() 58 | print() 59 | print("Vehicle speed: " + str(vehspd.data * 3.6) + " km/h") 60 | print("Simulation status: " + ("Running" if sim_status.data >= 61 | 0 else cm.status_dic.get(sim_status.data))) 62 | time.sleep(1) 63 | 64 | ``` 65 | 66 | ## How to use VDS: 67 | 68 | 1. First you need to create a VDS.cfg config file in Movie directory inside your project folder. 69 | Example cfg: 70 | ``` 71 | nViews 1 72 | # First camera 73 | 0 { 74 | Width 640 75 | Height 480 76 | Export {rgb} 77 | VPtOffset_x 3.1 78 | VPtOffset_y 0 79 | VPtOffset_z 1.1 80 | FrameRate 25 81 | } 82 | ``` 83 | 2. Run a Tes run and open IPGMovie. 84 | 3. Launch the following script 85 | 86 | ```python 87 | import cv2 88 | from pycarmaker import VDS 89 | 90 | # initalize VDS 91 | vds = VDS() 92 | # Connect 93 | vds.connect() 94 | # Read Images 95 | while(True): 96 | # Capture frame-by-frame 97 | frame = vds.read() 98 | frame = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR) 99 | # Display the resulting frame 100 | cv2.imshow('Frame', frame) 101 | # Press Q on keyboard to exit 102 | if cv2.waitKey(25) & 0xFF == ord('q'): 103 | break 104 | 105 | ``` 106 | 107 | -------------------------------------------------------------------------------- /examples/cm_ros_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import sys 3 | import os 4 | import logging 5 | from types import * 6 | import time 7 | from pycarmaker import CarMaker, Quantity 8 | import rospy 9 | from std_msgs.msg import Float32 10 | FORMAT = '[%(levelname)6s] %(module)10s: %(message)s' 11 | logging.basicConfig(format=FORMAT) 12 | 13 | if __name__ == "__main__": 14 | try: 15 | IP_ADDRESS = "localhost" 16 | PORT = 16660 17 | cm = CarMaker(IP_ADDRESS, PORT) 18 | cm.connect() 19 | car_speed = Quantity("Car.v", Quantity.FLOAT) 20 | car_yaw = Quantity("Car.Yaw", Quantity.FLOAT) 21 | driver_steer = Quantity("Driver.Steer.Ang", Quantity.FLOAT) 22 | # Initialize with negative speed to indicate that value was not read 23 | car_speed.data = -1.0 24 | car_yaw.data = -1.0 25 | driver_steer.data = -1.0 26 | # Subscribe (TCP socket need to be connected) 27 | cm.subscribe(car_speed) 28 | cm.subscribe(car_yaw) 29 | cm.subscribe(driver_steer) 30 | 31 | # Let's also read the simulation status (simulation status is not a quantity but a command 32 | # so the command parameter must be set to True) 33 | sim_status = Quantity("SimStatus", Quantity.INT, True) 34 | cm.subscribe(sim_status) 35 | 36 | # 6 - Read all subscribed quantities. In this example, vehicle speed and simulation status 37 | 38 | # For some reason, the first two reads will be incomplete and must be ignored 39 | # You will see 2 log errors like this: [ ERROR] CarMaker: Wrong read 40 | cm.read() 41 | cm.read() 42 | time.sleep(0.1) 43 | # ROS Publishers 44 | rospy.init_node('cmros', anonymous=True) 45 | speedpub = rospy.Publisher( 46 | '/cmros/car/speed', Float32, queue_size=10) 47 | yawpub = rospy.Publisher( 48 | '/cmros/car/yaw', Float32, queue_size=10) 49 | steerpub = rospy.Publisher( 50 | '/cmros/driver/steer/ang', Float32, queue_size=10) 51 | # Publishing Rate. 52 | rate = rospy.Rate(30) # 30hz 53 | print("Connecting") 54 | while not rospy.is_shutdown(): 55 | cm.read() 56 | print() 57 | print("Car speed: " + str(car_speed.data * 3.6) + " km/h") 58 | print("Car Yaw: " + str(car_yaw.data)) 59 | print("Steering Angle: " + str(driver_steer.data)) 60 | print("Simulation status: " + ("Running" if sim_status.data >= 61 | 0 else cm.status_dic.get(sim_status.data))) 62 | floatmsg = Float32() 63 | floatmsg.data = car_speed.data * 3.6 64 | speedpub.publish(floatmsg) 65 | floatmsg.data = car_yaw.data 66 | yawpub.publish(floatmsg) 67 | floatmsg.data = driver_steer.data 68 | steerpub.publish(floatmsg) 69 | rate.sleep() 70 | except rospy.ROSInterruptException: 71 | print("Shutting Down !. GoodBye") 72 | except Exception as e: 73 | print(e) 74 | print("Can't connect. Make sure CarMaker is running on port 16660") 75 | print("Run it first /opt/ipg/carmaker/linux64-8.1.1/bin/CM -cmdport 16660 ") 76 | -------------------------------------------------------------------------------- /examples/C/APO_Ex_01_Read_Quant.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | /* Include this if compiling suing Visual Studio */ 7 | #include 8 | #pragma comment(lib,"ws2_32.lib") //Winsock Library 9 | 10 | #include "apo.h" 11 | 12 | /* Step 1 13 | * Open CarMaker, load a TestRun and start the simulation 14 | */ 15 | 16 | /* Step 2 17 | * Execute this application and remember to restart it everytime you change the TestRun. 18 | * This code is an example and it is very simple. It does not handle changes in the Data Dictionary. 19 | */ 20 | 21 | int main() 22 | { 23 | const int channel = 2; 24 | 25 | printf("APO Example - Reading vehicle speed"); 26 | 27 | /* Initialize APO library */ 28 | int rsp = ApocInit(); 29 | printf("APO Init: %d\r\n", rsp); 30 | 31 | /* Servers query */ 32 | rsp = ApocQueryServers(2 * 1000, "localhost"); 33 | printf("APO QueryServers: %d\r\n", rsp); 34 | 35 | while (!ApocQueryDone()) 36 | { 37 | ApocPoll(); 38 | Sleep(100); 39 | } 40 | 41 | printf("APO Query Servers Done: %d servers found\r\n", ApocGetServerCount()); 42 | 43 | /* Show found servers */ 44 | for (int i = 0; i < ApocGetServerCount(); i++) 45 | { 46 | const tApoServerInfo* sinf = ApocGetServer(i); 47 | printf("Server: %s\r\n", sinf->Identity); 48 | } 49 | 50 | tApoSid sid = NULL; 51 | 52 | /* If you have CarMaker only as a server this will be always 0 */ 53 | sid = ApocOpenServer(0); 54 | 55 | if (sid == NULL) 56 | { 57 | printf("Open Server Error"); 58 | return 0; 59 | } 60 | 61 | /* Connect to server */ 62 | rsp = ApocConnect(sid, 1 << channel); 63 | printf("Connect: %d\r\n", rsp); 64 | 65 | while (ApocGetStatus(sid, NULL) == ApoConnPending) 66 | { 67 | ApocPoll(); 68 | ApoSleep_ms(100); 69 | printf("Waiting to connect\r\n"); 70 | } 71 | 72 | rsp = ApocGetStatus(sid, NULL); 73 | printf("Server status after connect: %s\r\n", ApoStrStatus(rsp)); 74 | 75 | if (!(ApocGetStatus(sid, NULL) & ApoConnUp)) 76 | { 77 | printf("Connection failed"); 78 | return 0; 79 | } 80 | 81 | rsp = ApocGetQuantCount(sid); 82 | printf("Number of quantities: %d\r\n", rsp); 83 | 84 | /* Show all quantities that starts with Car.v */ 85 | for (int i = 0; i < rsp; i++) 86 | { 87 | const tApoQuantInfo* qinf = ApocGetQuantInfo(sid, i); 88 | if (strncmp(qinf->Name, "Car.v", 5) == 0) 89 | { 90 | printf("%s [%s]\n", qinf->Name, qinf->Unit); 91 | } 92 | } 93 | 94 | /* Subscribe to Time and Car.v */ 95 | tApoSubscription subs[2]; 96 | subs[0].Name = "Car.v"; 97 | subs[1].Name = "Time"; 98 | 99 | rsp = ApocSubscribe(sid, 2, subs, 50, 1, 10, 0); 100 | printf("Subscribe of quantities: %d\r\n", rsp); 101 | 102 | rsp = ApocGetStatus(sid, NULL); 103 | printf("Server status after subscribe: %s\r\n", ApoStrStatus(rsp)); 104 | 105 | int seq; 106 | int msglen, msgch; 107 | char msgbuf[APO_ADMMAX]; 108 | 109 | while (1) 110 | { 111 | if (ApocWaitIO(50) == 1) { 112 | ApocPoll(); 113 | 114 | rsp = ApocGetStatus(sid, NULL); 115 | printf("Server status: %s - ", ApoStrStatus(rsp)); 116 | 117 | if (!(ApocGetStatus(sid, NULL) & ApoConnUp)) 118 | { 119 | ApoSleep_ms(50); 120 | break; 121 | } 122 | 123 | seq = ApocGetData(sid); 124 | printf("Seq: %03d - ", seq); 125 | 126 | while (seq > 0) 127 | { 128 | seq = ApocGetData(sid); 129 | } 130 | printf("Value of %s: Time: %6.3f: %3.3f", subs[0].Name, *((double*)subs[1].Ptr), *((float*)subs[0].Ptr)); 131 | 132 | printf("\n\r"); 133 | 134 | /* You must call this function even if not used. Please check APO.pdf section 2.1.2 Polling. */ 135 | ApocGetAppMsg(sid, &msgch, msgbuf, &msglen); 136 | ApoSleep_ms(50); 137 | } 138 | } 139 | 140 | ApocCloseServer(sid); 141 | return 0; 142 | } -------------------------------------------------------------------------------- /pycarmaker/CarMaker.py: -------------------------------------------------------------------------------- 1 | import socket 2 | import logging 3 | import numpy as np 4 | 5 | 6 | class Quantity(): 7 | FLOAT = 1.0 8 | INT = 1 9 | 10 | def __init__(self, name, q_type, command=False): 11 | self.name = name 12 | self.type = q_type 13 | self.command = command 14 | self.data = None 15 | 16 | if command == True: 17 | self.read_msg = self.name + "\r" 18 | else: 19 | self.read_msg = "expr {$Qu(" + self.name + ")}\r" 20 | 21 | 22 | class CarMaker(): 23 | status_dic = {-1: "Preprocessing", -2: "Idle", -3: "Postprocessing", -4: "Model Check", 24 | -5: "Driver Adaption", -6: "FATAL ERROR", -7: "Waiting for License", 25 | -8: "Simulation paused", -10: "Starting application", -11: "Simulink Initialization"} 26 | 27 | def __init__(self, ip, port, log_level=logging.INFO): 28 | self.logger = logging.getLogger("pycarmaker") 29 | self.logger.setLevel(log_level) 30 | 31 | self.ip = ip 32 | self.port = port 33 | 34 | self.socket = None 35 | self.quantities = [] 36 | 37 | self.logger.debug("pycarmaker init completed") 38 | 39 | def connect(self): 40 | # Open the TCP / IP port 41 | self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) 42 | 43 | # Connect to the CM TCP / IP port 16660 on localhost 44 | self.socket.connect((self.ip, self.port)) 45 | self.logger.info("TCP socket connected") 46 | 47 | def subscribe(self, quantity): 48 | self.quantities.append(quantity) 49 | 50 | if quantity.command == True: 51 | self.logger.info("Subscribe for command " + quantity.name + ": OK") 52 | return 53 | 54 | # Create message to subscribe to quantities with all quantities 55 | # previous subscribed 56 | msg = "" 57 | for q in self.quantities: 58 | if msg is "": 59 | msg = q.name 60 | else: 61 | msg += " " + q.name 62 | msg = "QuantSubscribe {" + msg + "}\r" 63 | self.logger.debug(msg) 64 | 65 | if (self.socket == None): 66 | self.logger.error("Not connected") 67 | return 68 | 69 | self.socket.send(msg.encode()) 70 | rsp = self.socket.recv(200) 71 | rsp = rsp.decode().split("\r\n\r\n") 72 | self.logger.info("Subscribe for quantity " + 73 | quantity.name + ": " + str(rsp)) 74 | # TODO Handle error 75 | 76 | def read(self): 77 | 78 | # By IPG recommendation, read one quantity at a time. 79 | for quantity in self.quantities: 80 | self.socket.send(quantity.read_msg.encode()) 81 | str_rx = self.socket.recv(300).decode() 82 | rx_list = str_rx.split("\r\n\r\n") 83 | self.logger.debug(rx_list) 84 | 85 | if (len(rx_list) != 2): 86 | self.logger.error("Wrong read") 87 | return 88 | 89 | if type(quantity.type) == type(Quantity.FLOAT): 90 | quantity.data = float(rx_list[0][1:]) 91 | elif type(quantity.type) == type(Quantity.INT): 92 | quantity.data = int(rx_list[0][1:]) 93 | else: 94 | self.logger.error("Unknwon type") 95 | 96 | def DVA_write(self, quantity, value, duration=-1, mode="Abs"): 97 | """ set the value of a variable using DVAWrite ... 98 | Parameters 99 | ---------- 100 | quant : Quantity 101 | Quantity to set. 102 | value : Float 103 | New quantity value 104 | duration : Float 105 | Duration in milliseconds 106 | mode : string 107 | One of Abs, Off, Fac, AbsRamp, ...; default Abs(olute Value) 108 | """ 109 | 110 | msg = "DVAWrite " + quantity.name + " " + \ 111 | str(value)+" "+str(duration)+" "+mode+"\r" 112 | self.socket.send(msg.encode()) 113 | rsp = self.socket.recv(200) 114 | rsp = rsp.decode().split("\r\n\r\n") 115 | self.logger.info("Write quantity " + 116 | quantity.name + ": " + str(rsp)) 117 | 118 | def DVA_release(self): 119 | """ Call this method when you are done using DVA """ 120 | self.send("DVAReleaseQuants\r") 121 | 122 | def send(self, msg): 123 | """ send the giving message to CarMaker 124 | Paramters 125 | --------- 126 | msg : string 127 | a string contains the message ending with \ r 128 | """ 129 | self.socket.send(msg.encode()) 130 | return self.socket.recv(200) 131 | 132 | 133 | class VDS: 134 | def __init__(self, ip="localhost", port=2210, log_level=logging.INFO): 135 | self.logger = logging.getLogger("pycarmaker") 136 | self.logger.setLevel(log_level) 137 | self.ip = ip 138 | self.port = port 139 | self.socket = None 140 | self.cameras = [] 141 | self.connected = False 142 | 143 | def connect(self): 144 | self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) 145 | self.socket.connect((self.ip, self.port)) 146 | data = self.socket.recv(64) 147 | if(data.decode().find("*IPGMovie") != -1): 148 | self.logger.info("IPG Movie is Connected...") 149 | self.connected = True 150 | 151 | def read(self): 152 | """ 153 | Read the streamed images. 154 | 155 | Returns 156 | ------- 157 | img : numpy array 158 | a numpy array representing the image 159 | 160 | """ 161 | if not self.connected: 162 | self.logger.error("Connect first by calling .connect()") 163 | return 164 | # Get Image header and fill data 165 | data = self.socket.recv(64) 166 | splitdata = data.decode().split(" ") 167 | imgtype = splitdata[2] 168 | img_size = splitdata[4] 169 | data_len = int(splitdata[5]) 170 | imag_h = int(img_size.split('x')[1]) 171 | image_w = int(img_size.split('x')[0]) 172 | lastdata = b'' 173 | size = 0 174 | while(size != data_len): 175 | data = self.socket.recv(1024) 176 | try: 177 | strdata = data.decode() 178 | if strdata[0] == '*' and strdata[1] == 'V': 179 | splitdata = data.decode().split(" ") 180 | imgtype = splitdata[2] 181 | img_size = splitdata[4] 182 | data_len = int(splitdata[5]) 183 | imag_h = int(img_size.split('x')[1]) 184 | image_w = int(img_size.split('x')[0]) 185 | lastdata = b'' 186 | size = 0 187 | continue 188 | except: 189 | pass 190 | lastdata += data 191 | size = np.frombuffer(lastdata, dtype=np.uint8).size 192 | datalist = np.frombuffer(lastdata, dtype=np.uint8) 193 | if(imgtype == "rgb"): 194 | img = datalist.reshape((imag_h, image_w, 3)) 195 | elif(imgtype == "grey"): 196 | img = datalist.reshape((imag_h, image_w)) 197 | else: 198 | self.logger.error("rgb and gray are supported for now") 199 | 200 | return img 201 | --------------------------------------------------------------------------------