├── .gitignore ├── ArduinoSketches ├── arduino_serial_led │ └── arduino_serial_led.ino ├── sketch_ATMega2560 │ └── sketch_ATMega2560.ino └── sketch_L298N_ArduinoUno │ └── sketch_L298N_ArduinoUno.ino ├── LICENSE.txt ├── README.md ├── launch └── serial_server_launch.py ├── package.xml ├── resource └── ros2_serial_interface ├── ros2_serial_interface ├── __init__.py ├── predef_timed_pub.py ├── pynput_key_pub.py ├── serial_server.py ├── terminal_key_pub.py ├── virtual_serial_client.py ├── virtual_serial_server.py ├── virtual_serial_setup.py └── virtual_serial_setup.sh ├── setup.cfg ├── setup.py ├── test ├── test_copyright.py ├── test_flake8.py └── test_pep257.py └── testing_instructions.md /.gitignore: -------------------------------------------------------------------------------- 1 | log/ 2 | build/ 3 | install/ 4 | __pycache__ 5 | .vscode -------------------------------------------------------------------------------- /ArduinoSketches/arduino_serial_led/arduino_serial_led.ino: -------------------------------------------------------------------------------- 1 | const int Led1 = 3; 2 | const int Led2 = 5; 3 | const int Led3 = 7; 4 | const int Led4 = 9; 5 | 6 | void setup() 7 | { 8 | pinMode(Led1, OUTPUT); 9 | pinMode(Led2, OUTPUT); 10 | pinMode(Led3, OUTPUT); 11 | pinMode(Led4, OUTPUT); 12 | 13 | Serial.begin(9600); 14 | while (! Serial); // Wait untilSerial is ready - Leonardo 15 | Serial.println("Enter 0 to turn off LED and 1 to turn on LED"); 16 | } 17 | 18 | void loop() 19 | { 20 | if (Serial.available()) 21 | { 22 | char ch = Serial.read(); 23 | if (ch == '0') 24 | { 25 | digitalWrite(Led1,LOW); 26 | digitalWrite(Led2,LOW); 27 | digitalWrite(Led3,LOW); 28 | digitalWrite(Led4,LOW); 29 | delay(10); 30 | Serial.println("All Leds Turned OFF"); 31 | } 32 | if (ch == '1') 33 | { 34 | digitalWrite(Led1,HIGH); 35 | delay(10); 36 | Serial.println("Led1 Turned ON"); 37 | } 38 | if (ch == '2') 39 | { 40 | digitalWrite(Led2,HIGH); 41 | delay(10); 42 | Serial.println("Led2 Turned ON"); 43 | } 44 | if (ch == '3') 45 | { 46 | digitalWrite(Led3,HIGH); 47 | delay(10); 48 | Serial.println("Led3 Turned ON"); 49 | } 50 | if (ch == '4') 51 | { 52 | digitalWrite(Led4,HIGH); 53 | delay(10); 54 | Serial.println("Led4 Turned ON"); 55 | } 56 | if (ch == '5') 57 | { 58 | digitalWrite(Led1,LOW); 59 | delay(10); 60 | Serial.println("Led1 Turned OFF"); 61 | } 62 | if (ch == '6') 63 | { 64 | digitalWrite(Led2,LOW); 65 | delay(10); 66 | Serial.println("Led2 Turned OFF"); 67 | } 68 | if (ch == '7') 69 | { 70 | digitalWrite(Led3,LOW); 71 | delay(10); 72 | Serial.println("Led3 Turned OFF"); 73 | } 74 | if (ch == '8') 75 | { 76 | digitalWrite(Led4,LOW); 77 | delay(10); 78 | Serial.println("Led4 Turned OFF"); 79 | } 80 | if (ch == '9') 81 | { 82 | digitalWrite(Led1,HIGH); 83 | digitalWrite(Led2,HIGH); 84 | digitalWrite(Led3,HIGH); 85 | digitalWrite(Led4,HIGH); 86 | delay(10); 87 | Serial.println("All Leds Turned ON"); 88 | } 89 | } 90 | } 91 | -------------------------------------------------------------------------------- /ArduinoSketches/sketch_ATMega2560/sketch_ATMega2560.ino: -------------------------------------------------------------------------------- 1 | //MAKE SURE YOU TURN BOTH ARDUINO AND PI ON AT THE SAME TIME! 2 | //IF YOU RESTART THE PI, RESTART THE ARDUINO! 3 | //Reason why is because Pi sends about 37 bytes to UART pins during boot time 4 | //This program delays setup for 11 seconds to prevent those bytes from being recieved 5 | 6 | 7 | //Make ten Motors 8 | //index 0 = Dir, index 1 = Step, index 2 = Enable 9 | 10 | int DriveMotor1[] = {52,12,14}; // back left wheel 11 | int DriveMotor2[] = {50,11,15}; // front left wheel 12 | int DriveMotor3[] = {40,6,66}; //back right wheel 13 | int DriveMotor4[] = {36,4,69}; //front right wheel 14 | 15 | int ArmMotor1[] = {46,9,17}; 16 | int ArmMotor2[] = {42,8,18}; 17 | int ArmMotor3[] = {44,7,19}; 18 | int ArmMotor4[] = {48,10,16}; 19 | int ArmMotor5[] = {38,5,67}; 20 | int ArmMotor6[] = {34,3,69}; 21 | 22 | const char move_forward = 'w'; 23 | const char move_backward = 's'; 24 | const char turn_left = 'a'; 25 | const char turn_right = 'd'; 26 | const char stop_move = 'x'; 27 | 28 | //Serial: Input data comes from USB CABLE; Read Serial for pulse calls 29 | 30 | void setup() { 31 | // put your setup code here, to run once: 32 | //Delay to avoid recieving the approximately 37 bytes the pi sents to the UART pins during boot 33 | delay(12000); 34 | //Up the speed of the internal timer 35 | TCCR1A = 0b00000011; // 10bit 36 | TCCR1B = 0b00001001; // x1 fast pwm 37 | //For loop to determine all Motor Pin Connections 38 | for(int i = 0; i<3; i++) 39 | { 40 | pinMode(DriveMotor1[i], OUTPUT); 41 | pinMode(DriveMotor2[i],OUTPUT); 42 | pinMode(DriveMotor3[i],OUTPUT); 43 | pinMode(DriveMotor4[i],OUTPUT); 44 | pinMode(ArmMotor1[i],OUTPUT); 45 | pinMode(ArmMotor2[i],OUTPUT); 46 | pinMode(ArmMotor3[i],OUTPUT); 47 | pinMode(ArmMotor4[i],OUTPUT); 48 | pinMode(ArmMotor5[i],OUTPUT); 49 | pinMode(ArmMotor6[i],OUTPUT); 50 | //Set all Motors to No Output 51 | digitalWrite(DriveMotor1[i],HIGH); 52 | digitalWrite(DriveMotor2[i],HIGH); 53 | digitalWrite(DriveMotor3[i],HIGH); 54 | digitalWrite(DriveMotor4[i],HIGH); 55 | digitalWrite(ArmMotor1[i],HIGH); 56 | digitalWrite(ArmMotor2[i],HIGH); 57 | digitalWrite(ArmMotor3[i],HIGH); 58 | digitalWrite(ArmMotor4[i],HIGH); 59 | digitalWrite(ArmMotor5[i],HIGH); 60 | digitalWrite(ArmMotor6[i],HIGH); 61 | } 62 | //Start up Serial Communication 63 | Serial.begin(9600); 64 | 65 | } 66 | 67 | void Start(int motor[], bool enab, bool direc) 68 | { 69 | if(enab == 1){ 70 | digitalWrite(motor[2],LOW); //For some reason, setting to LOW enables 71 | if(direc == 1) 72 | { 73 | digitalWrite(motor[0],LOW); 74 | } 75 | if(direc == 0) 76 | { 77 | digitalWrite(motor[0],HIGH); 78 | } 79 | 80 | analogWrite(motor[1], 127); 81 | } 82 | } 83 | 84 | 85 | //For some reason this modification to Start caused motor to start stuttering. 86 | //void Start(int motor[], bool direc) 87 | //{ 88 | // digitalWrite(motor[0],LOW); 89 | // if(direc == 1) 90 | // { 91 | // digitalWrite(motor[0],LOW); 92 | // } 93 | // if(direc == 0) 94 | // { 95 | // digitalWrite(motor[0],HIGH); 96 | // } 97 | // 98 | // analogWrite(motor[2], 127); 99 | //} 100 | 101 | void Stop(int motor[]) 102 | { 103 | digitalWrite(motor[2],HIGH); 104 | digitalWrite(motor[0],LOW); 105 | analogWrite(motor[1], 0); 106 | } 107 | 108 | /* Movement Functions: 109 | * Imagine a top-down view of the rover, with the front of it facing north 110 | * My assumption is that the motors are numbered going from left-to-right and front-to-back 111 | * If assumption is incorrect the values can easily be changed later 112 | * #1=Front-left wheel motor 113 | * #2=Front-right wheel motor 114 | * #3=Back-left wheel motor 115 | * #4=Back-right wheel motor 116 | * 117 | * NEW ASSIGNMENT OF MOTORS: 118 | * #1= Back-left wheel 119 | * #2= Front-left wheel 120 | * #3= Back-right wheel 121 | * #4= Front-right wheel 122 | */ 123 | 124 | void moveForward(){ 125 | Start(DriveMotor2, 1, 1); 126 | Start(DriveMotor4, 1, 1); 127 | Start(DriveMotor1, 1, 1); 128 | Start(DriveMotor3, 1, 1); 129 | } 130 | void moveBackward(){ 131 | Start(DriveMotor2, 1, 0); 132 | Start(DriveMotor4, 1, 0); 133 | Start(DriveMotor1, 1, 0); 134 | Start(DriveMotor3, 1, 0); 135 | } 136 | void turnLeft(){ 137 | Start(DriveMotor2, 1, 0); 138 | Start(DriveMotor4, 1, 1); 139 | Start(DriveMotor1, 1, 0); 140 | Start(DriveMotor3, 1, 1); 141 | } 142 | void turnRight(){ 143 | Start(DriveMotor2, 1, 1); 144 | Start(DriveMotor4, 1, 0); 145 | Start(DriveMotor1, 1, 1); 146 | Start(DriveMotor3, 1, 0); 147 | } 148 | void stopMoving(){ 149 | Stop(DriveMotor2); 150 | Stop(DriveMotor4); 151 | Stop(DriveMotor1); 152 | Stop(DriveMotor3); 153 | } 154 | void runCommand(char command) 155 | { 156 | 157 | if (command == move_forward) //move-forward 158 | { 159 | Serial.println("move-forward"); 160 | moveForward(); 161 | } 162 | else if (command == move_backward) //move-backward 163 | { 164 | Serial.println("move-backward"); 165 | moveBackward(); 166 | } 167 | else if (command == turn_left) //turn-left 168 | { 169 | Serial.println("turn-left"); 170 | turnLeft(); 171 | 172 | } 173 | else if (command == turn_right) //turn-right 174 | { 175 | Serial.println("turn-right"); 176 | turnRight(); 177 | 178 | } 179 | else if (command == stop_move) //stop-move 180 | { 181 | Serial.println("stop-move"); 182 | stopMoving(); 183 | } 184 | } 185 | 186 | void motorTest(int motor[], int ms_delay){ 187 | digitalWrite(motor[2],LOW); //Motor is Enabled 188 | digitalWrite(motor[0],HIGH); //Motor direction set HIGH 189 | analogWrite(motor[1], 127); //Motor step amount set to 127 190 | delay(ms_delay); //delay 191 | digitalWrite(motor[2],HIGH); //Motor is Disabled 192 | digitalWrite(motor[0],LOW); // Motor direction set LOW 193 | analogWrite(motor[1], 0); //Motor step amount set to 0 194 | delay(ms_delay); //delay 195 | } 196 | 197 | void loop() 198 | { 199 | // put your main code here, to run repeatedly: 200 | // Serial Communication Goes Here 201 | if(Serial.available()){ 202 | char command = Serial.read(); 203 | runCommand(command); 204 | } 205 | //motorTest(DriveMotor1, 2000); 206 | 207 | } 208 | -------------------------------------------------------------------------------- /ArduinoSketches/sketch_L298N_ArduinoUno/sketch_L298N_ArduinoUno.ino: -------------------------------------------------------------------------------- 1 | const int baud_r = 9600; 2 | const int fPinNo = 2; //first pin number 3 | const int lPinNo = 13; //last pin number 4 | const int ENA = 9; 5 | const int IN1 = 7; 6 | const int IN2 = 6; 7 | const int IN3 = 5; 8 | const int IN4 = 4; 9 | const int ENB = 3; 10 | const char move_forward = 'w'; 11 | const char move_backward = 's'; 12 | const char turn_left = 'a'; 13 | const char turn_right = 'd'; 14 | const char stop = 'x'; 15 | 16 | const int pwm_val = 140; 17 | int DCMotor1[] = {IN1,IN2, ENA}; 18 | int DCMotor2[] = {IN3,IN4, ENB}; 19 | 20 | //Serial: Input data comes from USB CABLE; Read Serial for pulse calls 21 | 22 | void setup() 23 | { 24 | for(int i=fPinNo; i <=lPinNo; i++){ 25 | pinMode(i, OUTPUT); 26 | } 27 | Serial.begin(baud_r); 28 | } 29 | 30 | void Start(int motor[], bool pin1, bool pin2, int pwm_val) 31 | { 32 | pin1 ? digitalWrite(motor[0], HIGH) : digitalWrite(motor[0], LOW); 33 | pin2 ? digitalWrite(motor[1], HIGH) : digitalWrite(motor[1], LOW); 34 | analogWrite(motor[2], pwm_val); 35 | } 36 | void Stop(int motor[]) 37 | { 38 | digitalWrite(motor[0], 0); 39 | digitalWrite(motor[1], 0); 40 | analogWrite(motor[2], 0); 41 | } 42 | 43 | void moveForward(){ 44 | Start(DCMotor1, 1, 0, pwm_val); 45 | Start(DCMotor2, 1, 0, pwm_val); 46 | } 47 | void moveBackward(){ 48 | Start(DCMotor1, 0, 1, pwm_val); 49 | Start(DCMotor2, 0, 1, pwm_val); 50 | } 51 | void turnLeft(){ 52 | Start(DCMotor1, 1, 0, 195); 53 | Start(DCMotor2, 0, 1, 195); 54 | } 55 | void turnRight(){ 56 | Start(DCMotor1, 0, 1, 195); 57 | Start(DCMotor2, 1, 0, 195); 58 | } 59 | void stopMoving(){ 60 | Stop(DCMotor1); 61 | Stop(DCMotor2); 62 | } 63 | void runCommand(char command) 64 | { 65 | 66 | if (command == move_forward) //move-forward 67 | { 68 | Serial.println("move-forward"); 69 | moveForward(); 70 | } 71 | else if (command == move_backward) //move-backward 72 | { 73 | Serial.println("move-backward"); 74 | moveBackward(); 75 | } 76 | else if (command == turn_left) //turn-left 77 | { 78 | Serial.println("turn-left"); 79 | turnLeft(); 80 | 81 | } 82 | else if (command == turn_right) //turn-right 83 | { 84 | Serial.println("turn-right"); 85 | turnRight(); 86 | 87 | } 88 | else if (command == stop) //stop 89 | { 90 | Serial.println("stop"); 91 | stopMoving(); 92 | } 93 | } 94 | 95 | void loop() 96 | { 97 | // put your main code here, to run repeatedly: 98 | // Serial Communication Goes Here 99 | char buffer[1]; 100 | if(Serial.available()){ 101 | Serial.readBytesUntil(',', buffer,1); 102 | runCommand(buffer[0]); 103 | } 104 | //motorTest(DriveMotor1, 2000); 105 | 106 | } 107 | -------------------------------------------------------------------------------- /LICENSE.txt: -------------------------------------------------------------------------------- 1 | MIT License 2 | Copyright (c) [2022] [Andrew Ponce] 3 | 4 | Permission is hereby granted, free of charge, to any person obtaining a copy 5 | of this software and associated documentation files (the "Software"), to deal 6 | in the Software without restriction, including without limitation the rights 7 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | copies of the Software, and to permit persons to whom the Software is 9 | furnished to do so, subject to the following conditions: 10 | 11 | The above copyright notice and this permission notice shall be included in all 12 | copies or substantial portions of the Software. 13 | 14 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | SOFTWARE. -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ROS2 Serial Interface 2 | 3 | ## What this is 4 | 5 | This interface is designed to serve as a serial communication bridge between two devices: the serial server and the serial client. 6 | 7 | The serial server is the device used to indirectly control the serial client via the use of ROS2 and serial communication 8 | 9 | The serial client is the device that listens for serial communication from the serial server. Based on what is communicated to it, the serial client performs a pre-defined action 10 | 11 | In theory any device capable of recieving data via serial communication can be the serial client or the serial server. However, the expectation is that the serial server is a device that runs/uses ROS2 (ie, like a Raspberry Pi), while the serial client is a device that doesn't run/use ROS2 (ie, like a microcontroller) 12 | 13 | Note that the serial client must be programmed with the intent of doing a particular action upon recieving a specific input from the serial server. For example, if the serial server sends the character 'w' to the serial client, the serial client must be programmed to listen for the character 'w'. Additionally, the serial server must be programmed knowing what the serial client will do once it recieves the character 'w'. 14 | 15 | The main intention of this project is to provide a basic framework/idea that anyone can use and modify to suit their particular project. The programs in this repository are designed to demonstrate what one CAN do with their project, not what one SHOULD do with their project. It is expected that those who wish to use these files will modify them heavily to suit their own individual needs 16 | 17 | ## Description of what each file does (in progress) 18 | 19 | predef_timed_pub.py: 20 | 21 | pynput_key_pub.py: 22 | 23 | serial_server.py: 24 | 25 | serial_server_launch.py: 26 | 27 | terminal_key_pub.py: 28 | 29 | virtual_serial_client.py: 30 | 31 | virtual_serial_server.py: 32 | 33 | virtual_serial_setup.py: 34 | 35 | ## Running these files 36 | 37 | To run predef_timed_pub.py: 38 | 39 | `ros2 run ros2_serial_interface predef_timed_pub` 40 | 41 | To run pynput_key_pub.py: 42 | 43 | `ros2 run ros2_serial_interface pynput_key_pub` 44 | 45 | To run serial_server.py: 46 | 47 | `ros2 run ros2_serial_interface serial_server` 48 | 49 | To run serial_server_launch.py: 50 | 51 | `ros2 launch ros2_serial_interface serial_server_launch.py` 52 | 53 | To run terminal_key_pub.py: 54 | 55 | `ros2 run ros2_serial_interface terminal_key_pub` 56 | 57 | To run virtual_serial_client.py: 58 | 59 | `ros2 run ros2_serial_interface virtual_serial_client` 60 | 61 | To run virtual_serial_server.py: 62 | 63 | `ros2 run ros2_serial_interface virtual_serial_server` 64 | 65 | To run virtual_serial_setup.py: 66 | 67 | `ros2 run ros2_serial_interface virtual_serial_setup` 68 | 69 | ## Testing these files 70 | 71 | For instructions on testing these files, see testing_instructions.md 72 | -------------------------------------------------------------------------------- /launch/serial_server_launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch_ros.actions import Node 3 | #if you run the launch file using ros2 launch after changing the parameter values, you need to rerun colcon build 4 | def generate_launch_description(): 5 | return LaunchDescription([ 6 | Node( 7 | package='ros2_serial_interface', 8 | executable='serial_server', 9 | name='serial_server_node', 10 | output='screen', 11 | emulate_tty=True, 12 | parameters=[ 13 | {'device': '/dev/ttyACM0'}, 14 | {'wheel_instructions_topic': 'wheel_instructions_topic'}, 15 | {'move_forward_lin_vel': 1.0}, 16 | {'move_backward_lin_vel': -1.0}, 17 | {'turn_left_ang_vel': 1.0}, 18 | {'turn_right_ang_vel': -1.0}, 19 | {'move_forward_cmd': 'w'}, 20 | {'move_backward_cmd': 's'}, 21 | {'turn_right_cmd': 'd'}, 22 | {'turn_left_cmd': 'a'}, 23 | {'stop_cmd': 'x'} 24 | ] 25 | ) 26 | ]) -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | ros2_serial_interface 5 | 0.0.0 6 | Ros2 Serial Interface 7 | andyp 8 | MIT 9 | 10 | 11 | 12 | rclpy 13 | pynput 14 | pyserial 15 | 16 | std_msgs 17 | serial 18 | 19 | 20 | ament_copyright 21 | ament_flake8 22 | ament_pep257 23 | python3-pytest 24 | 25 | 26 | ament_python 27 | 28 | 29 | -------------------------------------------------------------------------------- /resource/ros2_serial_interface: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AnrdyShmrdy/ros2_serial_interface/94e761d90dc035e8f4f88bdd0161e01f8334d300/resource/ros2_serial_interface -------------------------------------------------------------------------------- /ros2_serial_interface/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AnrdyShmrdy/ros2_serial_interface/94e761d90dc035e8f4f88bdd0161e01f8334d300/ros2_serial_interface/__init__.py -------------------------------------------------------------------------------- /ros2_serial_interface/predef_timed_pub.py: -------------------------------------------------------------------------------- 1 | #This program publishes predefined messages to a ROS2 topic at timed intervals 2 | import rclpy 3 | from rclpy.node import Node 4 | from geometry_msgs.msg import Twist 5 | import time 6 | def run_publisher(node): 7 | msg = Twist() 8 | msg.linear.x = 1.0 9 | node.publisher_.publish(msg) 10 | time.sleep(0.25) 11 | 12 | msg = Twist() 13 | msg.linear.x = -1.0 14 | node.publisher_.publish(msg) 15 | time.sleep(0.25) 16 | 17 | msg = Twist() 18 | msg.angular.z = 1.0 19 | node.publisher_.publish(msg) 20 | time.sleep(0.25) 21 | 22 | msg = Twist() 23 | msg.angular.z = -1.0 24 | node.publisher_.publish(msg) 25 | time.sleep(0.25) 26 | class Publisher(Node): 27 | def __init__(self): 28 | super().__init__('publisher') 29 | self.publisher_ = self.create_publisher(Twist, 'wheel_instructions_topic', 10) 30 | self.isFinished = False 31 | 32 | def main(args=None): 33 | rclpy.init(args=args) 34 | 35 | publisher = Publisher() 36 | run_publisher(publisher) 37 | 38 | # Destroy the node explicitly 39 | # (optional - otherwise it will be done automatically 40 | # when the garbage collector destroys the node object) 41 | publisher.destroy_node() 42 | rclpy.shutdown() 43 | 44 | 45 | if __name__ == '__main__': 46 | main() 47 | -------------------------------------------------------------------------------- /ros2_serial_interface/pynput_key_pub.py: -------------------------------------------------------------------------------- 1 | #This keyboard publisher gets input using the pynput library 2 | import rclpy 3 | from rclpy.node import Node 4 | from pynput.keyboard import Key, Listener, KeyCode 5 | # Handle Twist messages, linear and angular velocity 6 | from geometry_msgs.msg import Twist 7 | 8 | def run_publisher(node): 9 | while (node.isFinished == False): 10 | def onpress(key): 11 | msg = Twist() 12 | if isinstance(key, KeyCode): #lets us know if we can safely use key.char 13 | if key.char == 'w': 14 | msg.linear.x = 1.0 15 | node.publisher_.publish(msg) 16 | return False 17 | elif key.char == 's': 18 | msg.linear.x = -1.0 19 | node.publisher_.publish(msg) 20 | return False 21 | elif key.char == 'a': 22 | msg.angular.z = 1.0 23 | node.publisher_.publish(msg) 24 | return False 25 | elif key.char == 'd': 26 | msg.angular.z = -1.0 27 | node.publisher_.publish(msg) 28 | return False 29 | elif key == Key.esc: 30 | # Stop listener 31 | node.isFinished = True 32 | return False 33 | def onrelease(key): 34 | if isinstance(key, KeyCode): #lets us know if we can safely use key.char 35 | if key.char in {'w', 's', 'a', 'd'}: 36 | node.publisher_.publish(Twist()) #Publishes a Twist message with values of 0. This indicates stopping 37 | return False 38 | elif key == Key.esc: 39 | # Stop listener 40 | node.isFinished = True 41 | return False 42 | # initialising Listener object 43 | with Listener(suppress = True, on_press = onpress) as l: #TODO: Add back suppress = True parameter 44 | l.join() 45 | with Listener(suppress = True, on_release = onrelease) as l: #TODO: Add back suppress = True parameter 46 | l.join() 47 | class KeyboardPublisher(Node): 48 | def __init__(self): 49 | super().__init__('keyboard_publisher') 50 | self.publisher_ = self.create_publisher(Twist, 'wheel_instructions_topic', 10) 51 | self.isFinished = False 52 | 53 | def main(args=None): 54 | rclpy.init(args=args) 55 | 56 | keyboard_publisher = KeyboardPublisher() 57 | run_publisher(keyboard_publisher) 58 | 59 | # Destroy the node explicitly 60 | # (optional - otherwise it will be done automatically 61 | # when the garbage collector destroys the node object) 62 | keyboard_publisher.destroy_node() 63 | rclpy.shutdown() 64 | 65 | 66 | if __name__ == '__main__': 67 | main() 68 | -------------------------------------------------------------------------------- /ros2_serial_interface/serial_server.py: -------------------------------------------------------------------------------- 1 | # Serial server node 2 | import rclpy 3 | from rclpy.node import Node 4 | import serial 5 | 6 | # Handle Twist messages, linear and angular velocity 7 | from geometry_msgs.msg import Twist 8 | class SerialServer(Node): 9 | def __init__(self): 10 | super().__init__('serial_server') 11 | #Default Value declarations of ros2 params: 12 | self.declare_parameters( 13 | namespace='', 14 | parameters=[ 15 | ('device', '/dev/ttyACM0'), #device we are trasmitting to & recieving messages from 16 | ('wheel_instructions_topic', 'wheel_instructions_topic'), 17 | ('move_forward_lin_vel', 1.0), 18 | ('move_backward_lin_vel', -1.0), 19 | ('turn_left_ang_vel', 1.0), 20 | ('turn_right_ang_vel', -1.0), 21 | ('move_forward_cmd', 'w'), 22 | ('move_backward_cmd', 's'), 23 | ('turn_right_cmd', 'd'), 24 | ('turn_left_cmd', 'a'), 25 | ('stop_cmd', 'x'), 26 | ] 27 | ) 28 | self.wheel_topic_name = self.get_param_str('wheel_instructions_topic') 29 | self.device_name = self.get_param_str('device') 30 | self.forward_vel = self.get_param_float('move_forward_lin_vel') 31 | self.backward_vel = self.get_param_float('move_backward_lin_vel') 32 | self.turn_left_vel = self.get_param_float('turn_left_ang_vel') 33 | self.turn_right_vel = self.get_param_float('turn_right_ang_vel') 34 | self.move_forward_cmd = self.get_param_str('move_forward_cmd') 35 | self.move_backward_cmd = self.get_param_str('move_backward_cmd') 36 | self.turn_left_cmd = self.get_param_str('turn_left_cmd') 37 | self.turn_right_cmd = self.get_param_str('turn_right_cmd') 38 | self.stop_cmd = self.get_param_str('stop_cmd') 39 | self.ser = serial.Serial(self.device_name, 40 | 9600, #Note: Baud Rate must be the same in the arduino program, otherwise signal is not recieved! 41 | timeout=.1) 42 | 43 | self.subscriber = self.create_subscription(Twist, 44 | self.wheel_topic_name, 45 | self.serial_listener_callback, 46 | 10) 47 | self.subscriber # prevent unused variable warning 48 | self.ser.reset_input_buffer() 49 | def get_param_float(self, name): 50 | try: 51 | return float(self.get_parameter(name).get_parameter_value().double_value) 52 | except: 53 | pass 54 | def get_param_str(self, name): 55 | try: 56 | return self.get_parameter(name).get_parameter_value().string_value 57 | except: 58 | pass 59 | def send_cmd(self, cmd): 60 | print("Sending: " + cmd) 61 | self.ser.write(bytes(cmd,'utf-8')) 62 | def recieve_cmd(self): 63 | try: 64 | #try normal way of recieving data 65 | #sleep(.01) #sleep to allow time for serial_data to arrive. Otherwise this might return nothing 66 | line = self.ser.readline().decode('utf-8').rstrip() 67 | except: 68 | #if normal way doesn't work, try getting binary representation to see what went wrong 69 | line = str(self.ser.readline()) 70 | print("Recieved: " + line) 71 | def serial_listener_callback(self, msg): 72 | #NOTE: 73 | # For some reason, arduino sends back null byte (0b'' or Oxff) back after the first call to ser.write 74 | # If the statement in "try" executes when this happens, it causes this error which crashes the program: 75 | # UnicodeDecodeError: 'utf-8' codec can't decode byte 0xff in position 0: invalid start byte 76 | # To prevent this, I added the try-except blocks to prevent the program from crashing 77 | # If a null byte is sent, "except" is called which prevents the program from crashing 78 | """Move Forward""" 79 | if msg.linear.x == self.forward_vel: 80 | self.send_cmd(self.move_forward_cmd) 81 | self.recieve_cmd() 82 | """Move Backward""" 83 | if msg.linear.x == self.backward_vel: 84 | self.send_cmd(self.move_backward_cmd) 85 | self.recieve_cmd() 86 | """Turn Left""" 87 | if msg.angular.z == self.turn_left_vel: 88 | self.send_cmd(self.turn_left_cmd) 89 | self.recieve_cmd() 90 | """Turn Right""" 91 | if msg.angular.z == self.turn_right_vel: 92 | self.send_cmd(self.turn_right_cmd) 93 | self.recieve_cmd() 94 | """Stop""" 95 | if msg == Twist(): 96 | self.send_cmd(self.stop_cmd) 97 | self.recieve_cmd() 98 | 99 | 100 | def main(args=None): 101 | rclpy.init(args=args) 102 | serial_server = SerialServer() 103 | rclpy.spin(serial_server) 104 | 105 | if __name__ == '__main__': 106 | main() -------------------------------------------------------------------------------- /ros2_serial_interface/terminal_key_pub.py: -------------------------------------------------------------------------------- 1 | # Copyright 2011 Brown University Robotics. 2 | # Copyright 2017 Open Source Robotics Foundation, Inc. 3 | # All rights reserved. 4 | # 5 | # Software License Agreement (BSD License 2.0) 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions 9 | # are met: 10 | # 11 | # * Redistributions of source code must retain the above copyright 12 | # notice, this list of conditions and the following disclaimer. 13 | # * Redistributions in binary form must reproduce the above 14 | # copyright notice, this list of conditions and the following 15 | # disclaimer in the documentation and/or other materials provided 16 | # with the distribution. 17 | # * Neither the name of the Willow Garage nor the names of its 18 | # contributors may be used to endorse or promote products derived 19 | # from this software without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | # POSSIBILITY OF SUCH DAMAGE. 33 | 34 | # NOTE: Above copyright notice applies to portions of this code which originate from https://github.com/ros2/teleop_twist_keyboard 35 | 36 | #This keyboard publisher gets input directly from the linux/windows terminal 37 | import sys 38 | 39 | import geometry_msgs.msg 40 | import rclpy 41 | 42 | if sys.platform == 'win32': 43 | import msvcrt 44 | else: 45 | import termios 46 | import tty 47 | 48 | moveBindings = { 49 | 'w': (1, 0, 0, 0), 50 | 'a': (0, 0, 0, 1), 51 | 'd': (0, 0, 0, -1), 52 | 's': (-1, 0, 0, 0), 53 | } 54 | 55 | 56 | def getKey(settings): 57 | if sys.platform == 'win32': 58 | # getwch() returns a string on Windows 59 | key = msvcrt.getwch() 60 | else: 61 | tty.setraw(sys.stdin.fileno()) 62 | # sys.stdin.read() returns a string on Linux 63 | key = sys.stdin.read(1) 64 | termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) 65 | return key 66 | 67 | 68 | def saveTerminalSettings(): 69 | if sys.platform == 'win32': 70 | return None 71 | return termios.tcgetattr(sys.stdin) 72 | 73 | 74 | def restoreTerminalSettings(old_settings): 75 | if sys.platform == 'win32': 76 | return 77 | termios.tcsetattr(sys.stdin, termios.TCSADRAIN, old_settings) 78 | 79 | 80 | def main(): 81 | settings = saveTerminalSettings() 82 | 83 | rclpy.init() 84 | 85 | node = rclpy.create_node('keyboard_publisher') 86 | pub = node.create_publisher(geometry_msgs.msg.Twist, 'wheel_instructions_topic', 10) 87 | 88 | x = 0.0 89 | y = 0.0 90 | z = 0.0 91 | th = 0.0 92 | 93 | try: 94 | while True: 95 | key = getKey(settings) 96 | if key in moveBindings.keys(): 97 | x = float(moveBindings[key][0]) 98 | y = float(moveBindings[key][1]) 99 | z = float(moveBindings[key][2]) 100 | th = float(moveBindings[key][3]) 101 | else: 102 | x = 0.0 103 | y = 0.0 104 | z = 0.0 105 | th = 0.0 106 | if (key == '\x03'): 107 | break 108 | 109 | twist = geometry_msgs.msg.Twist() 110 | twist.linear.x = x 111 | twist.linear.y = y 112 | twist.linear.z = z 113 | twist.angular.x = 0.0 114 | twist.angular.y = 0.0 115 | twist.angular.z = th 116 | pub.publish(twist) 117 | 118 | except Exception as e: 119 | print(e) 120 | 121 | finally: 122 | twist = geometry_msgs.msg.Twist() 123 | twist.linear.x = 0.0 124 | twist.linear.y = 0.0 125 | twist.linear.z = 0.0 126 | twist.angular.x = 0.0 127 | twist.angular.y = 0.0 128 | twist.angular.z = 0.0 129 | pub.publish(twist) 130 | 131 | restoreTerminalSettings(settings) 132 | 133 | 134 | if __name__ == '__main__': 135 | main() 136 | -------------------------------------------------------------------------------- /ros2_serial_interface/virtual_serial_client.py: -------------------------------------------------------------------------------- 1 | from time import sleep 2 | import serial 3 | 4 | class VirtualSerialClient(): 5 | def __init__(self): 6 | self.transmitting_device = '/dev/ttyS2' #device we are trasmitting messages to 7 | self.recieving_device = '/dev/ttyS1' #device we are recieving messages from 8 | self.serial_send = serial.Serial(self.transmitting_device, 9 | 9600, #Note: Baud Rate must be the same in the arduino program, otherwise signal is not recieved! 10 | timeout=.1) 11 | self.serial_recieve = serial.Serial(self.recieving_device, 12 | 9600, #Note: Baud Rate must be the same in the arduino program, otherwise signal is not recieved! 13 | timeout=.1) 14 | def send_cmd(self, cmd): 15 | print("Sending: " + cmd) 16 | self.serial_send.write(bytes(cmd,'utf-8')) 17 | def recieve_cmd(self): 18 | #try normal way of recieving data 19 | sleep(.01) #sleep to allow time for serial_data to arrive. Otherwise this might return nothing 20 | self.line = self.serial_recieve.readline().decode('utf-8').rstrip() 21 | if(self.line == "w"): 22 | print("recieved the letter w") 23 | self.send_cmd("I have recieved w") 24 | elif(self.line == "a"): 25 | print("recieved the letter a") 26 | self.send_cmd("I have recieved a") 27 | elif(self.line == "s"): 28 | print("recieved the letter s") 29 | self.send_cmd("I have recieved s") 30 | elif(self.line == "d"): 31 | print("recieved the letter d") 32 | self.send_cmd("I have recieved d") 33 | elif(self.line == "x"): 34 | print("recieved the letter x") 35 | self.send_cmd("I have recieved x") 36 | 37 | def main(args=None): 38 | virtual_serial_client = VirtualSerialClient() 39 | while True: 40 | virtual_serial_client.recieve_cmd() 41 | 42 | if __name__ == '__main__': 43 | main() -------------------------------------------------------------------------------- /ros2_serial_interface/virtual_serial_server.py: -------------------------------------------------------------------------------- 1 | # Virtual Serial Server node 2 | from time import sleep 3 | import rclpy 4 | from rclpy.node import Node 5 | import serial 6 | 7 | # Handle Twist messages, linear and angular velocity 8 | from geometry_msgs.msg import Twist 9 | class VirtualSerialServer(Node): 10 | def __init__(self): 11 | super().__init__('virtual_serial_server') 12 | #Default Value declarations of ros2 params: 13 | self.declare_parameters( 14 | namespace='', 15 | parameters=[ 16 | ('transmitting_device', '/dev/ttyS0'), #device we are trasmitting messages to 17 | ('recieving_device', '/dev/ttyS3'), #device we are recieving messages from 18 | ('wheel_instructions_topic', 'wheel_instructions_topic'), 19 | ('move_forward_lin_vel', 1.0), 20 | ('move_backward_lin_vel', -1.0), 21 | ('turn_left_ang_vel', 1.0), 22 | ('turn_right_ang_vel', -1.0), 23 | ('move_forward_cmd', 'w'), 24 | ('move_backward_cmd', 's'), 25 | ('turn_right_cmd', 'd'), 26 | ('turn_left_cmd', 'a'), 27 | ('stop_cmd', 'x'), 28 | ] 29 | ) 30 | self.forward_vel = self.get_param_float('move_forward_lin_vel') 31 | self.backward_vel = self.get_param_float('move_backward_lin_vel') 32 | self.turn_left_vel = self.get_param_float('turn_left_ang_vel') 33 | self.turn_right_vel = self.get_param_float('turn_right_ang_vel') 34 | self.move_forward_cmd = self.get_param_str('move_forward_cmd') 35 | self.move_backward_cmd = self.get_param_str('move_backward_cmd') 36 | self.turn_left_cmd = self.get_param_str('turn_left_cmd') 37 | self.turn_right_cmd = self.get_param_str('turn_right_cmd') 38 | self.stop_cmd = self.get_param_str('stop_cmd') 39 | self.serial_send = serial.Serial(self.get_param_str('transmitting_device'), 40 | 9600, #Note: Baud Rate must be the same in the arduino program, otherwise signal is not recieved! 41 | timeout=.1) 42 | self.serial_recieve = serial.Serial(self.get_param_str('recieving_device'), 43 | 9600, #Note: Baud Rate must be the same in the arduino program, otherwise signal is not recieved! 44 | timeout=.1) 45 | 46 | self.subscriber = self.create_subscription(Twist, 47 | self.get_param_str('wheel_instructions_topic'), 48 | self.serial_listener_callback, 49 | 10) 50 | self.subscriber # prevent unused variable warning 51 | def get_param_float(self, name): 52 | try: 53 | return float(self.get_parameter(name).get_parameter_value().double_value) 54 | except: 55 | pass 56 | def get_param_str(self, name): 57 | try: 58 | return self.get_parameter(name).get_parameter_value().string_value 59 | except: 60 | pass 61 | def send_cmd(self, cmd): 62 | print("Sending: " + cmd) 63 | self.serial_send.write(bytes(cmd,'utf-8')) 64 | def recieve_cmd(self): 65 | try: 66 | #try normal way of recieving data 67 | sleep(.01) #sleep to allow time for serial_data to arrive. Otherwise this might return nothing 68 | line = self.serial_recieve.readline().decode('utf-8').rstrip() 69 | except: 70 | #if normal way doesn't work, try getting binary representation to see what went wrong 71 | line = str(self.serial_recieve.readline()) 72 | print("Recieved: " + line) 73 | def serial_listener_callback(self, msg): 74 | #NOTE: 75 | # For some reason, arduino sends back null byte (0b'' or Oxff) back after the first call to ser.write 76 | # If the statement in "try" executes when this happens, it causes this error which crashes the program: 77 | # UnicodeDecodeError: 'utf-8' codec can't decode byte 0xff in position 0: invalid start byte 78 | # To prevent this, I added the try-except blocks to prevent the program from crashing 79 | # If a null byte is sent, "except" is called which prevents the program from crashing 80 | """Move Forward""" 81 | if msg.linear.x == self.forward_vel: 82 | self.send_cmd(self.move_forward_cmd) 83 | self.recieve_cmd() 84 | """Move Backward""" 85 | if msg.linear.x == self.backward_vel: 86 | self.send_cmd(self.move_backward_cmd) 87 | self.recieve_cmd() 88 | """Turn Left""" 89 | if msg.angular.z == self.turn_left_vel: 90 | self.send_cmd(self.turn_left_cmd) 91 | self.recieve_cmd() 92 | """Turn Right""" 93 | if msg.angular.z == self.turn_right_vel: 94 | self.send_cmd(self.turn_right_cmd) 95 | self.recieve_cmd() 96 | """Stop""" 97 | if msg == Twist(): 98 | self.send_cmd(self.stop_cmd) 99 | self.recieve_cmd() 100 | 101 | 102 | def main(args=None): 103 | rclpy.init(args=args) 104 | virtual_serial_server = VirtualSerialServer() 105 | rclpy.spin(virtual_serial_server) 106 | 107 | if __name__ == '__main__': 108 | main() -------------------------------------------------------------------------------- /ros2_serial_interface/virtual_serial_setup.py: -------------------------------------------------------------------------------- 1 | from os import system 2 | def main(args=None): 3 | system('socat -d -d pty,link=/dev/ttyS0,raw,echo=0 pty,link=/dev/ttyS1,raw,echo=0 &') #setup serial server 4 | system('socat -d -d pty,link=/dev/ttyS2,raw,echo=0 pty,link=/dev/ttyS3,raw,echo=0 &') #setup serial client 5 | if __name__ == '__main__': 6 | main() -------------------------------------------------------------------------------- /ros2_serial_interface/virtual_serial_setup.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | socat -d -d pty,link=/dev/ttyS0,raw,echo=0 pty,link=/dev/ttyS1,raw,echo=0 & #setup serial server 3 | socat -d -d pty,link=/dev/ttyS2,raw,echo=0 pty,link=/dev/ttyS3,raw,echo=0 & #setup serial client -------------------------------------------------------------------------------- /setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script-dir=$base/lib/ros2_serial_interface 3 | [install] 4 | install-scripts=$base/lib/ros2_serial_interface 5 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | import os 3 | from glob import glob 4 | 5 | package_name = 'ros2_serial_interface' 6 | 7 | setup( 8 | name=package_name, 9 | version='0.0.0', 10 | packages=[package_name], 11 | data_files=[ 12 | ('share/ament_index/resource_index/packages', 13 | ['resource/' + package_name]), 14 | ('share/' + package_name, ['package.xml']), 15 | (os.path.join('share', package_name), glob('launch/*_launch.py')) 16 | ], 17 | install_requires=['setuptools'], 18 | zip_safe=True, 19 | maintainer='andyp', 20 | maintainer_email='andy.ponce@outlook.com', 21 | description='Ros2 Serial Interface', 22 | license='MIT', 23 | tests_require=['pytest'], 24 | entry_points={ 25 | 'console_scripts': [ 26 | 'predef_timed_pub = ros2_serial_interface.predef_timed_pub:main', 27 | 'pynput_key_pub = ros2_serial_interface.pynput_key_pub:main', 28 | 'serial_server = ros2_serial_interface.serial_server:main', 29 | 'terminal_key_pub = ros2_serial_interface.terminal_key_pub:main', 30 | 'virtual_serial_client = ros2_serial_interface.virtual_serial_client:main', 31 | 'virtual_serial_setup = ros2_serial_interface.virtual_serial_setup:main', 32 | 'virtual_serial_server = ros2_serial_interface.virtual_serial_server:main' 33 | ], 34 | }, 35 | ) 36 | -------------------------------------------------------------------------------- /test/test_copyright.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_copyright.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.copyright 20 | @pytest.mark.linter 21 | def test_copyright(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found errors' 24 | -------------------------------------------------------------------------------- /test/test_flake8.py: -------------------------------------------------------------------------------- 1 | # Copyright 2017 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_flake8.main import main_with_errors 16 | import pytest 17 | 18 | 19 | @pytest.mark.flake8 20 | @pytest.mark.linter 21 | def test_flake8(): 22 | rc, errors = main_with_errors(argv=[]) 23 | assert rc == 0, \ 24 | 'Found %d code style errors / warnings:\n' % len(errors) + \ 25 | '\n'.join(errors) 26 | -------------------------------------------------------------------------------- /test/test_pep257.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_pep257.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.linter 20 | @pytest.mark.pep257 21 | def test_pep257(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found code style errors / warnings' 24 | -------------------------------------------------------------------------------- /testing_instructions.md: -------------------------------------------------------------------------------- 1 | # Step-by-step testing instructions 2 | 3 | ## Testing with predef_timed_pub.py, serial_server.py, netcat, and socat 4 | 5 | First open three terminals and run "source install/setup.bash" in each of them 6 | 7 | In the first terminal, run: 8 | 9 | `nc -l 60001` 10 | 11 | This command sets up a netcat server to listen to messages on 127.0.0.1:60001 12 | 13 | In the second terminal, run these two commands: 14 | 15 | `socat pty,link=/dev/ttyACM0,raw tcp:127.0.0.1:60001&` 16 | 17 | `ros2 run ros2_serial_interface serial_server` 18 | 19 | The first command uses socat to setup /dev/ttyACM0 to forward data to 127.0.0.1:60001 and runs it in the background 20 | 21 | The second command runs the serial_server.py program, which listens to various ROS2 topics. By default it listens to the wheel_instructions_topic topic 22 | 23 | In the third terminal, run: 24 | 25 | `ros2 run ros2_serial_interface predef_timed_pub` 26 | 27 | This runs predef_timed_pub.py, which publishes various Twist messages to the wheel_instructions_topic topic 28 | 29 | In the second terminal, you'll see that the serial_server program reads the published messages from the third terminal 30 | 31 | In the first terminal, you'll see messages arrive that were sent by the second terminal 32 | 33 | Essentially, the serial server (second terminal) recieves the messages from predef_time_pub (third terminal). Based on the messages recieved, the serial_server sends the appropriate response via serial to /dev/ttyAMA0 (aka the serial client). The moment /dev/ttyACM0 recieves it, socat forwards it to 127.0.0.1:60001. Finally the netcat process that is listening for messages on 127.0.0.1:60001 displays these messages in the first terminal 34 | 35 | ## Testing with virtual_serial_server.py, virtual_serial_client.py, predef_timed_pub.py, and virtual_serial_setup 36 | 37 | This test essentially sets up two mock devices: 38 | 39 | ### virtual_serial_server 40 | 41 | - Like serial_server.py, this subscribes to the wheel_instructions_topic topic 42 | - Unlike serial_server.py, the port trasmitted to and the port received from are different 43 | - This transmits data to a serial port called '/dev/ttyS0' 44 | - The data sent to 'dev/ttyS0' gets forwarded to '/dev/ttyS1' using the socat utility 45 | - This recieves data from a serial port called '/dev/ttyS3' 46 | - The data recieved by '/dev/ttyS3' was forwarded from another serial port called '/dev/ttyS2' 47 | - Essentially, 'dev/ttyS0' sends data to '/dev/ttyS3' and '/dev/ttyS1' recieves data from '/dev/ttyS2' 48 | 49 | ### virtual_serial_client 50 | 51 | - This essentially communicates with the virtual_serial_server 52 | - This transmits data to a serial port called '/dev/ttyS2' 53 | - The data sent to 'dev/ttyS2' gets forwarded to '/dev/ttyS3' using the socat utility 54 | - This recieves data from a serial port called '/dev/ttyS1' 55 | - The data recieved by '/dev/ttyS1' was forwarded from another serial port called '/dev/ttyS0' 56 | - Essentially, 'dev/ttyS2' sends data to '/dev/ttyS1' and '/dev/ttyS3' recieves data from '/dev/ttyS0' 57 | 58 | First open three terminals and run "source install/setup.bash" in each of them 59 | 60 | In the first terminal, run the following commands: 61 | 62 | `ros2 run ros2_serial_interface virtual_serial_setup` 63 | 64 | `ros2 run ros2_serial_interface virtual_serial_server` 65 | 66 | The first command sets up /dev/ttyS0 to forward data to /dev/ttyS1 and then sets up /dev/ttyS2 to forward data to /dev/ttyS3 67 | 68 | The second command runs virtual_serial_server.py 69 | 70 | Like serial_server.py, virtual_serial_server.py will send serial data based on what data published to the wheel_instructions_topic topic 71 | 72 | Unlike serial_server.py however, virtual_serial_server.py sends serial data to /dev/ttyS0 and recieves serial data from /dev/ttyS3 73 | 74 | In the second terminal, run the following command: 75 | 76 | `ros2 run ros2_serial_interface virtual_serial_client` 77 | 78 | This command runs virtual_serial_client.py, which sets up a mock serial device to be used with virtual_serial_client.py. This mock device will recieve serial data from /dev/ttyS1 and send serial data to /dev/ttyS2 79 | 80 | In the third terminal, run the following command: 81 | 82 | `ros2 run ros2_serial_interface predef_timed_pub` 83 | 84 | This command runs predef_timed_pub.py, which will publishes data to the wheel_instructions_topic topic 85 | --------------------------------------------------------------------------------