├── .gitignore ├── INSTALL.md ├── LICENSE ├── README.md ├── REQUIREMENTS.md ├── hybrid_fuzzing.md └── src ├── .dockerignore ├── checker.py ├── cli_harness.py ├── config.py ├── constants.py ├── cov ├── .gitignore ├── Makefile ├── cov-fs.o.c └── cov-trace.o.c ├── coverage ├── .gitignore ├── __init__.py └── coverage_tool.py ├── executor.py ├── feedback.py ├── fuzzer.py ├── harness.py ├── idltest_replayer.py ├── inspector.py ├── librcl_apis ├── client.txt ├── expand_topic_name.txt ├── format.py ├── graph.txt ├── guard_condition.txt ├── init.txt ├── node.txt ├── publisher.txt ├── service.txt ├── subscriber.txt ├── time.txt ├── timer.txt ├── validate_topic_name.txt └── wait.txt ├── missions ├── church-fail.json ├── church-short.json ├── church.json ├── mission_generator.py ├── mission_generator2.py ├── mission_generator3.py ├── takeoff-push.json ├── takeoff-pushdown.json ├── takeoff-quick.json ├── takeoff-quickland.json ├── takeoff.json └── test.json ├── mutator.py ├── oracles ├── moveit.py ├── px4.py ├── rosidl.py ├── turtlebot.py └── turtlesim.py ├── policies ├── common │ ├── lifecycle_node.xml │ ├── node.xml │ └── node │ │ ├── logging.xml │ │ ├── parameters.xml │ │ └── time.xml ├── fuzzer.policy.xml └── sros2_node.policy.xml ├── px4_prep ├── blacklist.py ├── parameter_reference.md ├── params.pkl ├── params.txt └── preprocess_params.py ├── px4_ros_bridge.py ├── px4_utils.py ├── qos_override.yaml ├── rcl_harness.py ├── reproduce_mav.py ├── requirements.txt ├── ros2_fuzzer ├── __init__.py ├── process_handling.py ├── ros_basic_strategies.py ├── ros_commons.py ├── ros_fuzzer.py └── test.py ├── ros_to_mav.py ├── ros_utils.py ├── rosbag_parser.py ├── scheduler.py ├── state_monitor.py ├── tests ├── test_arith_float.py ├── test_arith_int.py ├── test_flips_bool.py ├── test_flips_byte.py ├── test_flips_float.py ├── test_flips_int.py ├── test_kinematics.py ├── test_tracer.py └── test_unicode.py ├── tracer.py ├── utils ├── clear_shm.sh ├── create_msg.py ├── msg_viewer.py ├── replay.py ├── reset_focus.sh ├── ros-radamsa.sh ├── set_focus.sh ├── speaker.py └── sros_init.sh └── watchlist ├── empty.json ├── idltest.json ├── moveit2.json ├── px4.json ├── turtlebot3.json └── turtlesim.json /.gitignore: -------------------------------------------------------------------------------- 1 | *.o 2 | *.lock 3 | *.swp 4 | *.swo 5 | *.pyc 6 | .gdb_history 7 | 8 | fuzzing_keys 9 | __pycache__ 10 | -------------------------------------------------------------------------------- /INSTALL.md: -------------------------------------------------------------------------------- 1 | 1. Install docker-ce 2 | 3 | Skip if you already have docker-ce installed on your system. 4 | The following is copied from https://docs.docker.com/engine/install/ubuntu/ 5 | 6 | ```sh 7 | $ sudo apt-get remove docker docker-engine docker.io containerd runc 8 | 9 | $ sudo apt-get update 10 | $ sudo apt-get install \ 11 | ca-certificates \ 12 | curl \ 13 | gnupg \ 14 | lsb-release 15 | 16 | $ sudo mkdir -p /etc/apt/keyrings 17 | $ curl -fsSL https://download.docker.com/linux/ubuntu/gpg | sudo gpg --dearmor -o /etc/apt/keyrings/docker.gpg 18 | $ echo \ 19 | "deb [arch=$(dpkg --print-architecture) signed-by=/etc/apt/keyrings/docker.gpg] https://download.docker.com/linux/ubuntu \ 20 | $(lsb_release -cs) stable" | sudo tee /etc/apt/sources.list.d/docker.list > /dev/null 21 | 22 | $ sudo apt-get update 23 | $ sudo apt-get install docker-ce docker-ce-cli containerd.io docker-compose-plugin 24 | ``` 25 | 26 | 2. Pull our docker image and tag it 27 | ``` 28 | $ docker pull ghcr.io/sslab-gatech/robofuzz:latest 29 | $ docker tag ghcr.io/sslab-gatech/robofuzz:latest robofuzz 30 | ``` 31 | (Due to the pre-compiled target robotic systems, the size of the image is 32 | approximately 20 GB, so docker pull might take a while.) 33 | 34 | 3. Run docker 35 | ``` 36 | $ xhost + 37 | $ docker run --rm -it -e DISPLAY=$DISPLAY -v /tmp/.X11-unix:/tmp/.X11-unix --name robofuzz robofuzz 38 | ``` 39 | 40 | 4. Check installation 41 | 42 | ```sh 43 | root@container_id:/robofuzz/src# ./fuzzer.py --help 44 | ``` 45 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 gts3.org (SSLab@Gatech) 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /REQUIREMENTS.md: -------------------------------------------------------------------------------- 1 | # Hardware requirements 2 | 3 | - Storage: at least 30 GB of free space 4 | - GPU: recommended, but not required 5 | - Any integrated graphics cards should suffice 6 | - A display device (monitor) 7 | - Gazebo simulator requires a display device to run on. Thus, headless 8 | servers cannot be used. 9 | 10 | 11 | # Software environments 12 | 13 | - Operating system: Ubuntu 20.04 14 | - X window system (e.g., xorg) 15 | - `echo $DISPLAY` should show a valid display name 16 | - Docker 17 | - `docker-ce` is recommended (https://docs.docker.com/engine/install/ubuntu/) 18 | 19 | 20 | # Robots 21 | 22 | We tested four robotic applications, and for two of them we utilized both the 23 | physical robots as well as the virtual models. If you intend to test the 24 | actual robots, you can purchase the following hardware and assemble them on 25 | your own: 26 | - TurtleBot3 burger 27 | - https://www.robotis.us/turtlebot-3-burger-us/ 28 | - PX4 drone - X500 kit by Holybro 29 | - https://shop.holybro.com/x500-kit_p1180.html 30 | 31 | For further information on the setup, please see `hybrid_fuzzing.md`. 32 | 33 | Otherwise, you can still perform tests with only the virtual models in the 34 | simulator, i.e., do a SITL (simulator-in-the-loop) testing. Everything for the 35 | SITL testing is included in the artifact. 36 | 37 | -------------------------------------------------------------------------------- /hybrid_fuzzing.md: -------------------------------------------------------------------------------- 1 | # HowTo: Hybrid Fuzzing 2 | Requirements for running RoboFuzz with an actual robot differs per robot. In 3 | this document, we provide instructions and a demonstration to shed light on 4 | the setup procedure for hybrid fuzzing. 5 | 6 | ## 1. Preparation (HW) 7 | First, prepare the physical robot. In this document, 8 | We will use [TurtleBot3 Burger (TB3)](https://www.robotis.us/turtlebot-3-burger-us/) 9 | as an example. 10 | 11 | Make sure that all parts are assembled, and that you can connect to the 12 | robot over the network (e.g., `ssh`). For ROS 2 messages to be delivered 13 | from the fuzzer (i.e., PC) to the robot and vice versa, both the PC and the 14 | robot should be on the same sub-network, e.g., connected to the same wifi 15 | router. 16 | 17 | We created a ssh key to automate the remote access and created two shell 18 | scripts, `run.sh` and `kill.sh`. `run.sh` sets up ROS 2 environment and 19 | launches TB3 via `ros2 launch`. `kill.sh` kills all ROS 2 processes. 20 | 21 | ```sh 22 | host_machine:~$ ssh -i path_to_robofuzz/src/keys/tb3 ubuntu@ip_addr_of_TB3 23 | 24 | ubuntu@ubuntu:~$ cat run.sh 25 | #!/bin/bash 26 | source /opt/ros/foxy/setup.bash 27 | source ~/turtlebot3_ws/install/setup.bash 28 | export TURTLEBOT3_MODEL=burger 29 | ros2 launch turtlebot3_bringup robot.launch.py 30 | 31 | ubuntu@ubuntu:~$ cat kill.sh 32 | #!/bin/bash 33 | kill -9 $(ps aux | grep ros | awk '{print $2}') 34 | ``` 35 | 36 | ## 2. Preparation (SW) 37 | On the software side, prepare the simulation following the [manual](https://emanual.robotis.com/docs/en/platform/turtlebot3/simulation/). 38 | 39 | ROS2 provides "topic remapping and namespacing", which allows you to group 40 | topics via namespace when you have multiple instances of robots. Put 41 | state-related topics under a namespace to differentiate them from the topics 42 | used by the physical robot. In the case of TB3, from `https://github.com/ROBOTIS-GIT/turtlebot3_simulations.git`, 43 | change the following to put simulation topics under the namespace `/sim`: 44 | ```sh 45 | diff --git a/turtlebot3_gazebo/models/turtlebot3_burger/model.sdf b/turtlebot3_gazebo/models/turtlebot3_burger/model.sdf 46 | index 562cac0..ac81e1a 100644 47 | --- a/turtlebot3_gazebo/models/turtlebot3_burger/model.sdf 48 | +++ b/turtlebot3_gazebo/models/turtlebot3_burger/model.sdf 49 | @@ -88,7 +88,7 @@ 50 | 51 | 52 | 53 | - 54 | + /sim 55 | ~/out:=imu 56 | 57 | 58 | @@ -156,7 +156,7 @@ 59 | 60 | 61 | 62 | - 63 | + /sim 64 | ~/out:=scan 65 | 66 | sensor_msgs/LaserScan 67 | @@ -365,7 +365,8 @@ 68 | 69 | 70 | 71 | - 72 | + /sim 73 | + /sim/cmd_vel:=/cmd_vel 74 | 75 | 76 | 30 77 | @@ -397,7 +398,7 @@ 78 | 79 | 80 | 81 | - 82 | + /sim 83 | ~/out:=joint_states 84 | 85 | 30 86 | ``` 87 | 88 | ## 3. Preparation (Fuzzer) 89 | 90 | Your watchlist should list the simulation topics as well as the actual 91 | robot's topics: 92 | ```sh 93 | $ cat watchlist/turtlebot3_hybrid.json 94 | { 95 | "/cmd_vel": "geometry_msgs/msg/Twist", 96 | "/imu": "sensor_msgs/msg/Imu", 97 | "/odom": "nav_msgs/msg/Odometry", 98 | "/scan": "sensor_msgs/msg/LaserScan", 99 | "/sim/imu": "sensor_msgs/msg/Imu", 100 | "/sim/odom": "nav_msgs/msg/Odometry", 101 | "/sim/scan": "sensor_msgs/msg/LaserScan" 102 | } 103 | ``` 104 | 105 | The IMU, odometry, and LiDAR scan data of the physical robot will be 106 | published to `/imu`, `/odom`, and `/scan` (by default) and the simulated one 107 | will do the same to `/sim/imu`, `/sim/odom`, and `/sim/scan`. 108 | All states of the physical robot and the simulated robot will be recorded. 109 | 110 | Write an oracle that reads from all aforementioned topics (see our oracle 111 | examples) and runs checks for the correctness properties you want to test. 112 | 113 | Create a harness function for starting both the simulator and the physical 114 | robot referring to `run_tb3_sitl()` and `run_tb3_hitl()` in `src/harness.py`. 115 | Register your harness function in `run_target` and `kill_target` of 116 | `src/fuzzer.py`, and you are all set! 117 | 118 | ## Video demo 119 | You can find a video demonstration of the TB3 bug found in the hybrid mode [here](https://youtu.be/MB5iCiYLBCI). 120 | 121 | ## Troubleshooting 122 | Please feel free to open a Github issue if you need additional assistance. 123 | 124 | -------------------------------------------------------------------------------- /src/.dockerignore: -------------------------------------------------------------------------------- 1 | bugs/ 2 | logs/ 3 | *.bag 4 | rawdata/ 5 | px4-coverage/ 6 | px4-coverage-withfeedback/ 7 | px4-coverage-withoutfeedback/ 8 | rosbags/ 9 | -------------------------------------------------------------------------------- /src/cli_harness.py: -------------------------------------------------------------------------------- 1 | import os 2 | import subprocess as sp 3 | import signal 4 | import time 5 | 6 | import rclpy 7 | from rclpy.node import Node 8 | from std_msgs.msg import String 9 | 10 | class CLISubscriber(Node): 11 | def __init__(self): 12 | super().__init__("cli_harness") 13 | 14 | # get cli command 15 | self.subscription = self.create_subscription( 16 | String, "cli_topic", self.cli_harness_callback, 10 17 | ) 18 | 19 | self.targets = [ 20 | "ros2 run examples_rclpy_minimal_publisher publisher_member_function", 21 | "ros2 run examples_rclcpp_minimal_publisher publisher_member_function", 22 | ] 23 | 24 | def cli_harness_callback(self, msg): 25 | print(f"[cli harness] Testing {msg.data}") 26 | 27 | # run target1, run cmd, store output1 28 | # run target2, run cmd, store output2 29 | for i, target_cmd in enumerate(self.targets): 30 | outfile = f"out-{i}" 31 | try: 32 | os.remove(outfile) 33 | except: 34 | pass 35 | 36 | pgrp = sp.Popen( 37 | target_cmd, 38 | shell=True, 39 | preexec_fn=os.setpgrp, 40 | stdout=sp.DEVNULL, 41 | stderr=sp.DEVNULL, 42 | ) 43 | time.sleep(3) 44 | 45 | cmd = f"ros2 param set /minimal_publisher use_sim_time {msg.data} >{outfile} 2>&1" 46 | os.system(cmd) 47 | 48 | if pgrp is not None: 49 | try: 50 | os.killpg(pgrp.pid, signal.SIGKILL) 51 | except: 52 | print(f"[-] could not kill {target_cmd}") 53 | 54 | try: 55 | os.remove(".waitlock") 56 | except: 57 | pass 58 | 59 | 60 | def main(): 61 | rclpy.init() 62 | 63 | node = CLISubscriber() 64 | 65 | rclpy.spin_once(node, timeout_sec=20) 66 | 67 | node.destroy_node() 68 | rclpy.shutdown() 69 | 70 | 71 | if __name__ == "__main__": 72 | main() 73 | -------------------------------------------------------------------------------- /src/config.py: -------------------------------------------------------------------------------- 1 | import enum 2 | import subprocess as sp 3 | import os 4 | from enum import Enum, auto 5 | 6 | class TestMode(Enum): 7 | GENERIC = auto() 8 | SROS2 = auto() 9 | PX4_SITL = auto() 10 | PX4_MISSION = auto() 11 | 12 | 13 | class RuntimeConfig: 14 | 15 | def __init__(self): 16 | config_path = os.path.abspath(__file__) 17 | self.src_dir = os.path.dirname(config_path) 18 | self.proj_root = os.path.dirname(self.src_dir) 19 | self.gcov_dir = os.path.join(self.src_dir, "coverage") 20 | 21 | self.replay = False 22 | 23 | self.persistent = False 24 | self.rospkg = None 25 | self.rosnode = None 26 | self.exec_cmd = None 27 | 28 | self.px4_sitl = False 29 | self.tb3_sitl = False 30 | self.tb3_hitl = False 31 | self.sros2 = False 32 | self.test_rcl = False 33 | 34 | def find_package_metadata(self): 35 | # read from provided meta file 36 | # or find from ros installation 37 | 38 | ros_pkg_cmd = "ros2 pkg prefix {}".format(self.rospkg) 39 | 40 | if self.rospkg is None or self.rosnode is None: 41 | print("[warning] ros_pkg or ros_node not provided.") 42 | print(" Using exec_cmd instead.") 43 | return -1 44 | 45 | proc = sp.Popen(ros_pkg_cmd.split(" "), stdout=sp.PIPE) 46 | out = proc.stdout.read().strip() 47 | if len(out) == 0: 48 | return -1 49 | self.pkg_prefix = str(out, "utf-8") 50 | 51 | try: 52 | pkg_xml = os.readlink(os.path.join(self.pkg_prefix, "share", 53 | self.rospkg, "package.xml")) 54 | except OSError: 55 | # if not built with --symlink-install 56 | pkg_xml = os.path.join(self.pkg_prefix, "share", self.rospkg, 57 | "package.xml") 58 | self.pkg_src_dir = os.path.join(os.path.dirname(pkg_xml), "src") 59 | 60 | self.ros_prefix = os.path.dirname(os.path.dirname(self.pkg_prefix)) 61 | self.pkg_cov_dir = os.path.join(self.ros_prefix, "build", self.rospkg, 62 | "CMakeFiles", "{}.dir".format(self.rosnode), "src") 63 | 64 | self.node_executable = os.path.join(self.pkg_prefix, "lib", 65 | self.rospkg, self.rosnode) 66 | 67 | return 0 68 | -------------------------------------------------------------------------------- /src/constants.py: -------------------------------------------------------------------------------- 1 | from enum import Enum, auto 2 | 3 | ros_time_types = ['time', 'duration'] 4 | ros_primitive_types = ['bool', 'byte', 'char', 'int8', 'uint8', 'int16', 5 | 'uint16', 'int32', 'uint32', 'int64', 'uint64', 6 | 'float32', 'float64', 7 | 'string'] 8 | ros_header_types = ['Header', 'std_msgs/Header', 'roslib/Header'] 9 | 10 | # fuzzing mode 11 | M_STARTOVER = 0 12 | M_STATEFUL = 1 13 | 14 | STRLEN_MAX = 16 15 | 16 | # colors 17 | RED = "\x1b[31m" 18 | GREEN = "\x1b[92m" 19 | END = "\x1b[0m" 20 | 21 | # lock files 22 | PUBLOCK = ".publish.lock" 23 | 24 | WATCHFLAG = ".watch" 25 | 26 | # IDL-related 27 | class BuiltInType(Enum): 28 | BOOL = auto() 29 | # BYTE = auto() # https://github.com/ros2/rosidl_runtime_py/issues/14 30 | CHAR = auto() 31 | FLOAT32 = auto() 32 | FLOAT64 = auto() 33 | INT8 = auto() 34 | UINT8 = auto() 35 | INT16 = auto() 36 | UINT16 = auto() 37 | INT32 = auto() 38 | UINT32 = auto() 39 | INT64 = auto() 40 | UINT64 = auto() 41 | STRING = auto() 42 | WSTRING = auto() 43 | 44 | 45 | class TypeExtension(Enum): 46 | BUILTIN = auto() # single built-in type 47 | FARRAY = auto() # array of fixed size 48 | BARRAY = auto() # bounded array of built-in types 49 | UBARRAY = auto() # unbounded array of built-in types 50 | 51 | # Cannot find the maximum allowed num of elements of an unbounded array 52 | # probably depends on the DDS implementation. 53 | # ref: https://answers.ros.org/question/299322/maximum-samples-and-unbounded-type-length-in-ros2-dds-entity/ 54 | FARRAY_BOUND = 64 55 | BARRAY_BOUND_MAX = 64 56 | UBARRAY_BOUND_MAX = 16384 # bigger numbers result in "arg list too long" error 57 | 58 | ### 59 | -------------------------------------------------------------------------------- /src/cov/.gitignore: -------------------------------------------------------------------------------- 1 | *.o 2 | -------------------------------------------------------------------------------- /src/cov/Makefile: -------------------------------------------------------------------------------- 1 | pass: 2 | clang -O3 -funroll-loops -g -fPIC -c cov-trace.o.c -o cov-trace.o 3 | clang -O3 -funroll-loops -g -fPIC -c cov-fs.o.c -o cov-fs.o 4 | 5 | turtle: 6 | colcon build --packages-select turtlesim --cmake-clean-cache --cmake-force-configure --cmake-arg -DCMAKE_BUILD_TYPE=Debug -DCMAKE_CXX_FLAGS="/home/seulbae/workspace/ros-clang/cov-trace.o -fsanitize-coverage=trace-pc-guard" --symlink-install 7 | -------------------------------------------------------------------------------- /src/cov/cov-fs.o.c: -------------------------------------------------------------------------------- 1 | // trace-pc-guard-cb.cc 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | const char* covfile_path; 12 | 13 | // This callback is inserted by the compiler as a module constructor 14 | // into every DSO. 'start' and 'stop' correspond to the 15 | // beginning and end of the section with the guards for the entire 16 | // binary (executable or DSO). The callback will be called at least 17 | // once per DSO and may be called multiple times with the same parameters. 18 | void __sanitizer_cov_trace_pc_guard_init(uint32_t *start, 19 | uint32_t *stop) { 20 | static uint64_t N; // Counter for the guards. 21 | if (start == stop || *start) return; // Initialize only once. 22 | printf("INIT: %p %p\n", start, stop); 23 | for (uint32_t *x = start; x < stop; x++) 24 | *x = ++N; // Guards should start from 1. 25 | 26 | covfile_path = getenv("COVFILE_PATH"); 27 | if (!covfile_path) { 28 | printf("COVFILE_PATH envvar is not set. Defaulting to /tmp/cov\n"); 29 | covfile_path = "/tmp/cov"; 30 | } 31 | int ret = unlink(covfile_path); 32 | if (ret < 0) { 33 | printf("[-] could not remove %s\n", covfile_path); 34 | } 35 | } 36 | 37 | // This callback is inserted by the compiler on every edge in the 38 | // control flow (some optimizations apply). 39 | // Typically, the compiler will emit the code like this: 40 | // if(*guard) 41 | // __sanitizer_cov_trace_pc_guard(guard); 42 | // But for large functions it will emit a simple call: 43 | // __sanitizer_cov_trace_pc_guard(guard); 44 | void __sanitizer_cov_trace_pc_guard(uint32_t *guard) { 45 | if (!*guard) return; // Duplicate the guard check. 46 | // If you set *guard to 0 this code will not be called again for this edge. 47 | // Now you can get the PC and do whatever you want: 48 | // store it somewhere or symbolize it and print right away. 49 | // The values of `*guard` are as you set them in 50 | // __sanitizer_cov_trace_pc_guard_init and so you can make them consecutive 51 | // and use them to dereference an array or a bit vector. 52 | void *PC = __builtin_return_address(0); 53 | /* char PcDescr[1024]; */ 54 | // This function is a part of the sanitizer run-time. 55 | // To use it, link with AddressSanitizer or other sanitizer. 56 | 57 | char buf[20] = {0, }; 58 | int numbytes = snprintf(buf, 20, "%lx ", (unsigned long)PC); 59 | /* __sanitizer_symbolize_pc(PC, "%p %F %L", PcDescr, sizeof(PcDescr)); */ 60 | int fd = open(covfile_path, O_CREAT | O_RDWR | O_APPEND, 0644); 61 | ssize_t size = write(fd, buf, numbytes); 62 | /* printf("guard: %p %x \n", guard, *guard); */ 63 | int ret = close(fd); 64 | } 65 | -------------------------------------------------------------------------------- /src/cov/cov-trace.o.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | char* shm; 8 | int has_shm = 0; 9 | 10 | #define CONST_PRIO 101 11 | __attribute__((constructor(CONST_PRIO))) void __ros_fuzzer_init(void) { 12 | FILE *fp = fopen("/tmp/shmid", "r"); 13 | size_t len; 14 | char* line = NULL; 15 | ssize_t read = getline(&line, &len, fp); 16 | fclose(fp); 17 | 18 | int shmid = atoi(line); 19 | /* printf("shm id: %d\n", shmid); */ 20 | 21 | shm = (char *)shmat(shmid, 0, 0); 22 | if (shm == (void*)-1) { 23 | printf("cannot attach to shm id %d\n", shmid); 24 | abort(); 25 | } 26 | printf("Attached ti shm id %d at: %p\n", shmid, shm); 27 | has_shm = 1; 28 | } 29 | 30 | void __sanitizer_cov_trace_pc_guard_init(uint32_t* start, uint32_t* stop) { 31 | static uint64_t N; // Counter for the guards. 32 | if (start == stop || *start) return; // Initialize only once. 33 | /* printf("INIT: %p %p\n", start, stop); */ 34 | for (uint32_t *x = start; x < stop; x++) 35 | *x = ++N; // Guards should start from 1. 36 | } 37 | 38 | void __sanitizer_cov_trace_pc_guard(uint32_t *guard) { 39 | if (has_shm) { 40 | shm[*guard % 65536]++; 41 | } 42 | } 43 | -------------------------------------------------------------------------------- /src/coverage/.gitignore: -------------------------------------------------------------------------------- 1 | *.gcov 2 | -------------------------------------------------------------------------------- /src/coverage/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sslab-gatech/RoboFuzz/d4199bb834242441056fc10f0f51055cf7910416/src/coverage/__init__.py -------------------------------------------------------------------------------- /src/coverage/coverage_tool.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | 3 | import os 4 | import time 5 | import pprint 6 | 7 | 8 | def clear_gcda(config): 9 | # 10 | # .gcda files need to be removed before executing node binary 11 | # 12 | 13 | cnt = 0 14 | for filename in os.listdir(config.pkg_cov_dir): 15 | if filename.endswith(".gcda"): 16 | cnt += 1 17 | os.unlink(os.path.join(config.pkg_cov_dir, filename)) 18 | 19 | # print("[*] unlinked {} files".format(cnt)) 20 | 21 | 22 | def run_gcov(config): 23 | gcov_options = "--branch-probabilities" 24 | gcov_options += " --branch-counts" 25 | gcov_options += " --intermediate-format" 26 | 27 | cov_obj_files = [filename for filename in os.listdir(config.pkg_cov_dir) 28 | if filename.endswith(".gcno")] 29 | 30 | cov_report_files = [] 31 | for cov_obj_file in cov_obj_files: 32 | src_filename = os.path.splitext(cov_obj_file)[0] 33 | src_absname = os.path.join(config.pkg_src_dir, src_filename) 34 | if not os.path.exists(src_absname): 35 | print("[-] couldn't find src file") 36 | continue 37 | 38 | obj_absname = os.path.join(config.pkg_cov_dir, cov_obj_file) 39 | 40 | cmd = "gcov {} {} {} {} > /dev/null 2>&1".format(gcov_options, src_absname, 41 | "--object-file", obj_absname) 42 | 43 | os.chdir(config.gcov_dir) 44 | os.system(cmd) 45 | os.chdir(config.src_dir) 46 | cov_report_files.append(src_filename + ".gcov") 47 | 48 | return cov_report_files 49 | 50 | 51 | def check_cov(config, cov_report_files, covmap): 52 | updated = False 53 | for report_file in cov_report_files: 54 | # print(report_file) 55 | with open(os.path.join(config.gcov_dir, report_file), "r") as fp: 56 | cov_report = fp.readlines() 57 | 58 | do_branch = False 59 | do_populate = False 60 | branch_updated = False 61 | bu_lineno = 0 62 | bu_index = [] 63 | for line in cov_report: 64 | line_tokens = line.strip().split(":") 65 | tag = line_tokens[0] 66 | info = line_tokens[1] 67 | # print(tag, info) 68 | 69 | if tag == "file": 70 | file_absname = info 71 | file_name = info.split("/")[-1] 72 | # print("SOURCE:", file_name) 73 | if file_name in covmap: 74 | line_map = covmap[file_name] 75 | else: 76 | line_map = {} 77 | covmap[file_name] = line_map 78 | 79 | # elif tag == "function": 80 | # line_no = int(info.split(",")[0]) 81 | # exec_cnt = info.split(",")[1] 82 | # func_name = info.split(",")[2].rstrip() 83 | # if len(func_map) == 0: 84 | # line_map = {} 85 | # func_map[line_no] = line_map 86 | # else: 87 | # line_map = func_map[line_no] 88 | 89 | elif tag == "lcount": 90 | # process and show updated branches 91 | if branch_updated: 92 | with open(file_absname, "r") as fp: 93 | lines = fp.readlines() 94 | try: 95 | code = "".join(lines[bu_lineno-4:bu_lineno-1]) 96 | except IndexError: 97 | code = "" 98 | code += "\x1b[91m" + lines[bu_lineno-1] + "\x1b[0m" 99 | try: 100 | code += "".join(lines[bu_lineno:bu_lineno+2]) 101 | except IndexError: 102 | pass 103 | 104 | print("[cov] new branch @{}".format(bu_lineno)) 105 | print(code) 106 | updated = True 107 | cstr = "" 108 | for index, elem in enumerate(line_map[bu_lineno][1]): 109 | if index in bu_index: 110 | s = "\x1b[92m" + str(elem) + "\x1b[0m" 111 | else: 112 | s = str(elem) 113 | cstr += s 114 | if index < len(line_map[bu_lineno][1]) -1: 115 | cstr += ", " 116 | lstr = "[{}, [{}]]".format(line_map[bu_lineno][0], cstr) 117 | print(old_branch_info) 118 | print(lstr) 119 | 120 | branch_updated = False 121 | bu_lineno = 0 122 | bu_index = [] 123 | do_branch = True 124 | do_populate = False 125 | 126 | line_no = int(info.split(",")[0]) 127 | line_count = int(info.split(",")[1]) 128 | if line_no in line_map: 129 | prev_line_count = line_map[line_no][0] 130 | # line_map[line_no][0] += line_count # accumulate line cnt 131 | if line_count > 0: 132 | line_map[line_no][0] += line_count 133 | if prev_line_count == 0 and line_map[line_no][0] > 0: 134 | with open(file_absname, "r") as fp: 135 | lines = fp.readlines() 136 | try: 137 | code = "".join(lines[line_no-4:line_no-1]) 138 | except IndexError: 139 | code = "" 140 | code += "\x1b[91m" + lines[line_no-1] + "\x1b[0m" 141 | try: 142 | code += "".join(lines[line_no:line_no+2]) 143 | except IndexError: 144 | pass 145 | print("[cov] new line @{}".format(line_no)) 146 | print(code) 147 | updated = True 148 | else: 149 | # line_cov_arr = [line_count, []] 150 | if line_count == 0: 151 | line_cov_arr = [0, []] 152 | else: 153 | line_cov_arr = [line_count, []] 154 | line_map[line_no] = line_cov_arr 155 | old_branch_info = str(line_map[line_no]) 156 | 157 | elif tag == "branch": 158 | line_no = int(info.split(",")[0]) 159 | branch_type = info.split(",")[1] 160 | line_cov_arr = line_map[line_no] 161 | if do_branch: 162 | # print("process branch") 163 | if len(line_cov_arr[1]) == 0: 164 | do_populate = True 165 | # print("populate branch array") 166 | do_branch = False 167 | b_index = 0 168 | 169 | if branch_type == "taken": 170 | b_type_code = 1 171 | elif branch_type == "nottaken": 172 | b_type_code = 0 173 | else: # notexec 174 | b_type_code = -1 175 | 176 | if do_populate: 177 | line_map[line_no][1].append(b_type_code) 178 | else: 179 | old_branch_exec = line_map[line_no][1][b_index] 180 | if b_type_code > old_branch_exec and b_type_code == 1: 181 | branch_updated = True 182 | bu_lineno = line_no 183 | bu_index.append(b_index) 184 | # print("[cov] new branch:", line_no, 185 | # line_map[line_no][1], "@", b_index) 186 | line_map[line_no][1][b_index] = b_type_code 187 | 188 | b_index += 1 189 | 190 | if updated: 191 | return time.time() 192 | else: 193 | return 0.0 194 | 195 | 196 | def get_cov_stat(last_update, covmap): 197 | print("*** COVERAGE - {:.2f}s since new coverage ***".format(time.time() - last_update)) 198 | for filename in covmap: 199 | lmap = covmap[filename] 200 | total_lines = len(lmap) 201 | exec_lines = 0 202 | total_branches = 0 203 | exec_branches = 0 204 | 205 | for lineno in lmap: 206 | line_info = lmap[lineno] 207 | if line_info[0] > 0: 208 | exec_lines += 1 209 | branch_info = line_info[1] 210 | total_branches += len(branch_info) 211 | for branch in branch_info: 212 | if branch == 1: 213 | exec_branches += 1 214 | 215 | print("- FILE {}".format(filename)) 216 | print(" - Lines : {} / {} ({:.2f}%)".format(exec_lines, total_lines, 217 | (exec_lines / float(total_lines)) * 100)) 218 | print(" - Branch: {} / {} ({:.2f}%)".format(exec_branches, 219 | total_branches, (exec_branches / float(total_branches)) * 100)) 220 | 221 | 222 | if __name__ == "__main__": 223 | pkg_name = "turtlesim" 224 | node_name = "turtlesim_node" 225 | 226 | ros_prefix= "/home/seulbae/workspace/ros-security/ros2_dashing" 227 | 228 | src_dir = "src/ros/ros_tutorials/turtlesim/src" 229 | bin_dir = "build/turtlesim" 230 | cov_dir = "build/turtlesim/CMakeFiles/turtlesim_node.dir/src" 231 | 232 | gcov_options = "--branch-probabilities --branch-counts --intermediate-format" 233 | # gcov_options = "--branch-probabilities --branch-counts" 234 | # gcov_options = "--branch-probabilities --intermediate-format" 235 | 236 | all_src_files = os.listdir(os.path.join(ros_prefix, src_dir)) 237 | # print(all_src_files) 238 | 239 | cov_obj_files = [filename for filename in os.listdir(os.path.join(ros_prefix, 240 | cov_dir)) if filename.endswith(".gcno")] 241 | 242 | for cov_obj_file in cov_obj_files: 243 | print(cov_obj_file) 244 | a = input(" ") 245 | src_filename = os.path.splitext(cov_obj_file)[0] 246 | src_absname = os.path.join(ros_prefix, src_dir, src_filename) 247 | if not os.path.exists(src_absname): 248 | print("[-] couldn't find src file") 249 | continue 250 | obj_absname = os.path.join(ros_prefix, cov_dir, cov_obj_file) 251 | 252 | cmd = "gcov {} {} {} {}".format(gcov_options, src_absname, 253 | "--object-file", obj_absname) 254 | 255 | # print(cmd) 256 | os.system(cmd) 257 | 258 | with open(src_filename + ".gcov", "r") as fp: 259 | cov_report = fp.readlines() 260 | 261 | executed_lines = [] 262 | for line in cov_report: 263 | line_tokens = line.strip().split(":") 264 | tag = line_tokens[0] 265 | info = line_tokens[1] 266 | 267 | if tag == "lcount": 268 | line_no = info.split(",")[0] 269 | line_count = info.split(",")[1] 270 | if int(line_count) > 0: 271 | executed_lines.append(int(line_no)) 272 | if tag == "branch": 273 | line_no = info.split(",")[0] 274 | branch_type = info.split(",")[1] 275 | print("branch", line_no, branch_type) 276 | pass 277 | 278 | -------------------------------------------------------------------------------- /src/executor.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from threading import Thread 4 | import time 5 | import pickle 6 | from enum import Enum, auto 7 | import subprocess as sp 8 | import json 9 | import signal 10 | import shutil 11 | 12 | import constants as c 13 | import ros_utils 14 | 15 | import rclpy 16 | from rclpy.qos import QoSProfile, HistoryPolicy, DurabilityPolicy 17 | from rosidl_runtime_py import message_to_ordereddict, message_to_yaml 18 | 19 | from std_msgs.msg import UInt64 20 | from std_srvs.srv import Empty 21 | 22 | 23 | class ExecMode(Enum): 24 | SINGLE = auto() 25 | SEQUENCE = auto() 26 | 27 | 28 | class Executor: 29 | def __init__(self, fuzzer): 30 | self.fuzzer = fuzzer 31 | 32 | def prep_execution(self, msg_type_class, topic_name): 33 | # prepare execution 34 | # create publisher 35 | 36 | self.topic_name = topic_name 37 | self.msg_type_class = msg_type_class 38 | 39 | self.msg_typestr = ros_utils.get_msg_typename_from_class( 40 | self.msg_type_class 41 | ) 42 | 43 | # self.fuzzer.pub = self.fuzzer.node_ptr.create_publisher( 44 | # msg_type_class, topic_name, 100 45 | # ) 46 | 47 | # qos = QoSProfile( 48 | # history=HistoryPolicy.KEEP_ALL, 49 | # durability=DurabilityPolicy.TRANSIENT_LOCAL, 50 | # ) 51 | # self.fuzzer.state_pub = self.fuzzer.node_ptr.create_publisher( 52 | # Bool, "listen_flag", qos_profile=qos 53 | # ) 54 | 55 | # self.fuzzer.cli_start_monitor = self.fuzzer.node_ptr.create_client( 56 | # Empty, 57 | # "start_monitor" 58 | # ) 59 | # while not self.fuzzer.cli_start_monitor.wait_for_service(timeout_sec=1.0): 60 | # print("[executor] waiting for start_monitor service") 61 | 62 | # # self.fuzzer.req_start_monitor = Empty.Request() 63 | 64 | # self.fuzzer.cli_stop_monitor = self.fuzzer.node_ptr.create_client( 65 | # Empty, 66 | # "stop_monitor" 67 | # ) 68 | # while not self.fuzzer.cli_stop_monitor.wait_for_service(timeout_sec=1.0): 69 | # print("[executor] waiting for stop_monitor service") 70 | 71 | # # self.fuzzer.req_stop_monitor = Empty.Request() 72 | 73 | def clear_execution(self): 74 | return 75 | self.fuzzer.pub.destroy() 76 | self.fuzzer.state_pub.destroy() 77 | 78 | def request_start_monitor(self): 79 | future = self.fuzzer.cli_start_monitor.call_async(Empty.Request()) 80 | 81 | # this shouldn't timeout, but it sometime does. 82 | rclpy.spin_until_future_complete( 83 | self.fuzzer.node_ptr, future, timeout_sec=None 84 | ) 85 | 86 | print("[executor] monitor started") 87 | # res = future.result() # don't have to check 88 | 89 | def request_stop_monitor(self): 90 | print("[executor] request stop monitor") 91 | future = self.fuzzer.cli_stop_monitor.call_async(Empty.Request()) 92 | rclpy.spin_until_future_complete(self.fuzzer.node_ptr, future) 93 | # spin_thread = Thread(target=rclpy.spin, args=(self.fuzzer.node_ptr,)) 94 | # spin_thread.start() 95 | # res = self.fuzzer.cli_stop_monitor.call(Empty.Request()) 96 | print("[executor] monitor stopped") 97 | # res = future.result() # don't have to check 98 | 99 | def start_watching(self): 100 | print("[executor] start watching") 101 | 102 | with open("/tmp/start_ts", "w") as f: 103 | f.write(str(int(str(time.time()).replace(".", "")))) 104 | 105 | # sp.call( 106 | # [ 107 | # "ros2", 108 | # "topic", 109 | # "pub", 110 | # "--once", 111 | # "/listen_flag", 112 | # "std_msgs/UInt64", 113 | # "{data: " + str(int(str(time.time()).replace(".", ""))) + "}", 114 | # ], 115 | # stdout=sp.DEVNULL, 116 | # stderr=sp.DEVNULL, 117 | # ) 118 | 119 | return 120 | 121 | state_msg = Bool() 122 | state_msg.data = True 123 | 124 | # deal with lossy environments by repeating msgs 125 | for i in range(5): 126 | self.fuzzer.state_pub.publish(state_msg) 127 | time.sleep(0.5) 128 | 129 | def stop_watching(self): 130 | print("[executor] stop watching") 131 | 132 | with open("/tmp/end_ts", "w") as f: 133 | f.write(str(int(str(time.time()).replace(".", "")))) 134 | 135 | # sp.call( 136 | # [ 137 | # "ros2", 138 | # "topic", 139 | # "pub", 140 | # "--once", 141 | # "/listen_flag", 142 | # "std_msgs/UInt64", 143 | # "{data: " + str(int(str(time.time()).replace(".", ""))) + "}", 144 | # ], 145 | # stdout=sp.DEVNULL, 146 | # stderr=sp.DEVNULL, 147 | # ) 148 | 149 | return 150 | 151 | state_msg = Bool() 152 | state_msg.data = False 153 | 154 | # deal with lossy environments by repeating msgs 155 | for i in range(5): 156 | self.fuzzer.state_pub.publish(state_msg) 157 | time.sleep(0.5) 158 | 159 | def start_rosbag(self, exec_cnt): 160 | watchlist_file = self.fuzzer.config.watchlist 161 | 162 | bag_dir = f"states-{exec_cnt}.bag" 163 | with open(watchlist_file, "r") as fp: 164 | data = fp.read() 165 | 166 | try: 167 | shutil.rmtree(bag_dir) 168 | except: 169 | print("[executor] could not remove previous bag dir") 170 | 171 | rosbag_cmd = f"ros2 bag record -o {bag_dir} --include-hidden-topics --qos-profile-overrides-path qos_override.yaml " 172 | watchlist = json.loads(data) 173 | for target in watchlist: 174 | rosbag_cmd += target + " " 175 | rosbag_cmd += "listen_flag" 176 | 177 | self.rosbag_pgrp = sp.Popen( 178 | rosbag_cmd, 179 | shell=True, 180 | preexec_fn=os.setpgrp, 181 | stdout=sp.PIPE, 182 | stderr=sp.PIPE, 183 | ) 184 | print("[executor] started ros2 bag recording") 185 | 186 | # need time for topics to be discovered 187 | time.sleep(1) 188 | 189 | def kill_rosbag(self): 190 | try: 191 | print("[executor] killing ros2 bag") 192 | os.killpg(self.rosbag_pgrp.pid, signal.SIGINT) 193 | except ProcessLookupError as e: 194 | print("[-] ros2 bag killpg error:", e) 195 | except AttributeError as e: 196 | print("[-] ros2 bag not initialized:", e) 197 | 198 | def save_msg_to_queue(self, msg, frame, subframe): 199 | if self.fuzzer.config.replay: 200 | return 201 | 202 | md = message_to_ordereddict(msg) 203 | queue_file = os.path.join( 204 | self.fuzzer.config.queue_dir, "msg-{}-{}".format(frame, subframe) 205 | ) 206 | 207 | pickle.dump(md, open(queue_file, "wb")) 208 | 209 | def do_pre_execution(self, pre_exec_list): 210 | """ 211 | Execute tasks (if any) that need to precede the main execution 212 | (e.g., arming) 213 | 214 | Parameters 215 | ---------- 216 | pre_exec_list : list of tuples (function, (args)) 217 | """ 218 | 219 | if pre_exec_list is not None: 220 | for (function, args) in pre_exec_list: 221 | function(*args) 222 | 223 | def do_post_execution(self, post_exec_list): 224 | """ 225 | Execute tasks (if any) that need to precede the main execution 226 | (e.g., arming) 227 | 228 | Parameters 229 | ---------- 230 | post_exec_list : list of tuples (function, (args)) 231 | """ 232 | 233 | if post_exec_list is not None: 234 | for (function, args) in post_exec_list: 235 | function(*args) 236 | 237 | def do_ros2_pub(self, msg): 238 | try: 239 | os.remove("out") 240 | except: 241 | pass 242 | 243 | try: 244 | os.remove("err") 245 | except: 246 | pass 247 | 248 | out = open("out", "w") 249 | err = open("err", "w") 250 | sp.call( 251 | [ 252 | "ros2", 253 | "topic", 254 | "pub", 255 | "--once", 256 | self.topic_name, 257 | self.msg_typestr, 258 | message_to_yaml(msg), 259 | ], 260 | stdout=out, 261 | stderr=err, 262 | ) 263 | out.close() 264 | err.close() 265 | 266 | if os.path.getsize("err") > 0: 267 | with open("err", "r") as f: 268 | e = "".join(f.readlines()) 269 | return e 270 | 271 | def execute( 272 | self, 273 | mode, 274 | msg_list, 275 | frame, 276 | frequency, 277 | repeat=1, 278 | pre_exec_list=None, 279 | post_exec_list=None, 280 | pub_function=None, 281 | wait_lock=None, 282 | ): 283 | """ 284 | Run target, 285 | publish a message (or a sequence of messages) to a topic, 286 | and then kill target. 287 | 288 | Parameters 289 | ---------- 290 | mode : ExecMode 291 | msg_list : [message_type] 292 | frame : datetime 293 | Timeframe the message is mutated 294 | frequency : float 295 | Rate (Hz) at which the given messages are published 296 | repeat : int 297 | Number of desired repetitions of the entire message (default: 1) 298 | Can be used for measuring repeatability by setting repeat > 1 299 | pre_exec_list : [function] 300 | List of tasks that need to precede the main execution 301 | (e.g., px4 arming) 302 | pub_function : function 303 | Custom method to publish messages. If None specified, use roscli. 304 | """ 305 | 306 | exec_cnt = 0 307 | period = 1 / frequency 308 | 309 | print("[executor] len(msg_list):", len(msg_list)) 310 | 311 | if mode == ExecMode.SINGLE: 312 | assert len(msg_list) == 1 313 | 314 | elif mode == ExecMode.SEQUENCE: 315 | assert len(msg_list) > 1 316 | 317 | while True: # outer loop: repetition of entire messages 318 | if exec_cnt == repeat: 319 | print(f"finished repeating {repeat} times") 320 | break 321 | 322 | print(f"repeating {exec_cnt}-th sequence") 323 | 324 | if not self.fuzzer.config.persistent: 325 | # target should already be running in persistent mode 326 | self.fuzzer.run_target( 327 | self.fuzzer.config.rospkg, 328 | self.fuzzer.config.rosnode, 329 | self.fuzzer.config.exec_cmd, 330 | ) 331 | 332 | self.do_pre_execution(pre_exec_list) 333 | 334 | # fp = open(c.WATCHFLAG, "w") 335 | # fp.close() 336 | self.start_rosbag(exec_cnt) 337 | self.start_watching() 338 | 339 | # self.request_start_monitor() 340 | # print("[executor] waiting for state monitor", end="") 341 | # while not os.path.isfile(c.PUBLOCK): 342 | # print(".", end="", flush=True) 343 | # time.sleep(0.1) 344 | # print() 345 | 346 | pub_failure = False 347 | if mode == ExecMode.SINGLE: 348 | subframe = time.time() 349 | msg = msg_list[0] 350 | 351 | self.save_msg_to_queue(msg, frame, subframe) 352 | 353 | # TODO: publishing through API is terribly unreliable. 354 | # Switch to externally calling "ros2 topic pub" as it 355 | # automatically creates a new hidden publisher. 356 | 357 | # byte doesn't work well with message_to_yaml() 358 | # reported the issue here: 359 | # https://github.com/ros2/rosidl_runtime_py/issues/14 360 | if pub_function: 361 | pub_function(msg) 362 | 363 | else: 364 | ret = self.do_ros2_pub(msg) 365 | 366 | if ret: 367 | pub_failure = True 368 | pub_failure_msg = ret 369 | print(f"[executor] failed to publish: {ret}") 370 | 371 | 372 | # self.fuzzer.pub.publish(msg) 373 | time.sleep(period) 374 | 375 | elif mode == ExecMode.SEQUENCE: 376 | for i, msg in enumerate(msg_list): 377 | # print("{}-th msg in sequence".format(i)) 378 | subframe = time.time() 379 | 380 | self.save_msg_to_queue(msg, frame, subframe) 381 | 382 | if pub_function: 383 | pub_function(msg) 384 | 385 | else: 386 | ret = self.do_ros2_pub(msg) 387 | 388 | if ret: 389 | pub_failure = True 390 | pub_failure_msg = ret 391 | print(f"[executor] failed to publish: {ret}") 392 | 393 | # self.fuzzer.pub.publish(msg) 394 | time.sleep(period) 395 | 396 | if wait_lock: 397 | f = open(wait_lock, "w") 398 | f.close() 399 | 400 | while True: 401 | time.sleep(0.2) 402 | if not os.path.isfile(wait_lock): 403 | break 404 | 405 | exec_cnt += 1 406 | 407 | # self.request_stop_monitor() 408 | # try: 409 | # print("removing watchflag") 410 | # os.remove(c.WATCHFLAG) 411 | # except: 412 | # print("cannot remove watchflag file") 413 | # pass 414 | 415 | # # wait for the monitor to dump file 416 | # while not os.path.isfile("states.pkl"): 417 | # time.sleep(0.1) 418 | self.do_post_execution(post_exec_list) 419 | 420 | self.stop_watching() 421 | self.kill_rosbag() 422 | 423 | if not self.fuzzer.config.persistent: 424 | # should not kill target in persistent mode 425 | self.fuzzer.kill_target() 426 | 427 | if pub_failure: 428 | return (1, pub_failure_msg) 429 | else: 430 | return (0, "") 431 | -------------------------------------------------------------------------------- /src/feedback.py: -------------------------------------------------------------------------------- 1 | from enum import Enum, auto 2 | 3 | class FeedbackType(Enum): 4 | """define feedback types (e.g., which direction to favor)""" 5 | INC = auto() # favor if value increases 6 | DEC = auto() # favor if value decreases 7 | ZERO = auto() # favor if closer to zero 8 | TARGET = auto() # favor if closer to target value 9 | 10 | 11 | class Feedback: 12 | """ 13 | Generic class defining a feedback. 14 | In practice, "feedback" will be a vector of Feedback instances. 15 | """ 16 | 17 | def __init__(self, name, feed_type, default_value=None): 18 | self.name = name # name of the feedback attribute 19 | self.feed_type = feed_type 20 | self.default_value = default_value 21 | self.value = default_value 22 | self.prev_interesting_value = None 23 | self.interesting_value = None # current interesting value 24 | self.target_value = None # only if type: TARGET 25 | 26 | def set_target(self, target_value): 27 | assert (self.feed_type == FeedbackType.TARGET) 28 | self.target_value = target_value 29 | 30 | def update_value(self, value): 31 | # self.prev_value = self.value 32 | self.value = value 33 | 34 | def update_interesting(self, init=False): 35 | if init: 36 | self.prev_interesting_value = self.value 37 | self.interesting_value = self.value 38 | else: 39 | self.prev_interesting_value = self.interesting_value 40 | self.interesting_value = self.value 41 | 42 | def reset(self): 43 | """reset feedback instance when a bug is found""" 44 | self.value = self.default_value 45 | self.prev_interesting_value = None 46 | self.interesting_value = None 47 | 48 | def is_interesting(self): 49 | # transition from the initial state to any state is interesting 50 | if self.interesting_value is None: 51 | self.update_interesting(init=True) 52 | return False 53 | 54 | # compare current value to the last value that was considered 55 | # interesting, and update if the current value is more interesting 56 | if self.feed_type == FeedbackType.INC: 57 | if self.interesting_value < self.value: 58 | self.update_interesting() 59 | return True 60 | else: 61 | return False 62 | 63 | elif self.feed_type == FeedbackType.DEC: 64 | if self.interesting_value > self.value: 65 | self.update_interesting() 66 | return True 67 | else: 68 | return False 69 | 70 | elif self.feed_type == FeedbackType.ZERO: 71 | if abs(self.interesting_value - 0) > abs(self.value - 0): 72 | self.update_interesting() 73 | return True 74 | else: 75 | return False 76 | 77 | elif self.feed_type == FeedbackType.TARGET: 78 | if abs(self.interesting_value - self.target_value) > abs(self.value - self.interesting_value): 79 | self.update_interesting() 80 | return True 81 | else: 82 | return False 83 | 84 | return False 85 | 86 | def quantify_interestingness(self): 87 | # if available, quantify and return the "interestingness" 88 | pass 89 | 90 | -------------------------------------------------------------------------------- /src/harness.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | import signal 4 | import subprocess as sp 5 | import time 6 | import json 7 | 8 | import rclpy 9 | from rosidl_runtime_py import message_to_ordereddict, set_message_fields 10 | 11 | 12 | def run_px4_stack_proc(): 13 | px4_root = "/home/seulbae/workspace/ros-security/targets/PX4-Autopilot" 14 | 15 | model = "iris" 16 | world = "fuzzing" 17 | bin_server = "gzserver" 18 | opt_server = os.path.join( 19 | px4_root, f"Tools/sitl_gazebo/worlds/{world}.world" 20 | ) 21 | cmd1 = f"{bin_server} {opt_server}" 22 | 23 | bin_model = "gz model" 24 | opt_model = "--spawn-file={} --model-name={} -x {} -y {} -z {}".format( 25 | os.path.join( 26 | px4_root, f"Tools/sitl_gazebo/models/{model}/{model}.sdf" 27 | ), 28 | f"{model}", 29 | "1.01", 30 | "0.98", 31 | "0.83", 32 | ) 33 | cmd2 = f"{bin_model} {opt_model}" 34 | 35 | env_client = os.environ.copy() 36 | env_client["GAZEBO_PLUGIN_PATH"] = os.path.join( 37 | px4_root, "build/px4_sitl_rtps/build_gazebo" 38 | ) 39 | env_client["GAZEBO_MODEL_PATH"] = os.path.join( 40 | px4_root, "Tools/sitl_gazebo/models" 41 | ) 42 | env_client["LD_LIBRARY_PATH"] = os.path.join( 43 | px4_root, "build/px4_sitl_rtps/build_gazebo" 44 | ) 45 | bin_client = "gzclient" 46 | opt_client = "--gui-client-plugin libgazebo_user_camera_plugin.so" 47 | cmd3 = f"{bin_client} {opt_client}" 48 | 49 | env_px4 = os.environ.copy() 50 | env_px4["PX4_SIM_MODEL"] = "iris" 51 | bin_px4 = os.path.join(px4_root, "build/px4_sitl_rtps/bin/px4") 52 | etc_dir = os.path.join(px4_root, "build/px4_sitl_rtps", "etc") 53 | rcs = os.path.join(px4_root, "build/px4_sitl_rtps/etc/init.d-posix/rcS") 54 | data_dir = os.path.join(px4_root, "test_data") 55 | opt_px4 = f"{etc_dir} -s {rcs} -t {data_dir}" 56 | 57 | cwd = os.getcwd() 58 | wd = os.path.join(px4_root, "build/px4_sitl_rtps/tmp/rootfs") 59 | os.chdir(wd) 60 | cmd4 = f"{bin_px4} {opt_px4}" 61 | 62 | print(cmd1) 63 | pgrp1 = sp.Popen(cmd1, shell=True, preexec_fn=os.setpgrp) 64 | time.sleep(5) 65 | 66 | print(cmd2) 67 | pgrp2 = sp.Popen(cmd2, shell=True, preexec_fn=os.setpgrp) 68 | time.sleep(5) 69 | 70 | print(cmd3) 71 | pgrp3 = sp.Popen(cmd3, shell=True, preexec_fn=os.setpgrp, env=env_client) 72 | time.sleep(5) 73 | 74 | print(cmd4) 75 | pgrp4 = sp.Popen(cmd4, shell=True, preexec_fn=os.setpgrp, env=env_px4) 76 | time.sleep(5) 77 | 78 | os.killpg(pgrp1.pid, signal.SIGKILL) 79 | os.killpg(pgrp2.pid, signal.SIGKILL) 80 | os.killpg(pgrp3.pid, signal.SIGKILL) 81 | os.killpg(pgrp4.pid, signal.SIGKILL) 82 | 83 | 84 | def run_px4_stack_sh(proj_dir): 85 | px4_dir = os.path.join(proj_dir, "targets", "PX4-Autopilot") 86 | # use below if measuring coverage: 87 | # px4_dir = "/home/seulbae/workspace/px4-cov" 88 | devnull = "2>&1 > /dev/null" 89 | 90 | simulator = "gazebo" 91 | model = "iris" 92 | 93 | cmd_list = [ 94 | "PX4_SITL_WORLD=church", 95 | os.path.join(px4_dir, "Tools", "sitl_run.sh"), 96 | os.path.join(px4_dir, "build", "px4_sitl_rtps", "bin", "px4"), 97 | "none", 98 | simulator, 99 | model, 100 | "none", 101 | px4_dir, 102 | os.path.join(px4_dir, "build", "px4_sitl_rtps"), 103 | devnull, 104 | ] 105 | cmd = " ".join(cmd_list) 106 | # print("command:", cmd) 107 | 108 | proc = sp.Popen(cmd, shell=True, stdout=sp.DEVNULL, stderr=sp.DEVNULL) 109 | return proc 110 | 111 | 112 | def run_tb3_sitl(proj_dir): 113 | tb3_dir = os.path.join(proj_dir, "targets", "turtlebot3_ws") 114 | devnull = "2>&1 > /dev/null" 115 | 116 | ros_pkg = "turtlebot3_gazebo" 117 | sim_map = "turtlebot3_world.launch.py" 118 | 119 | cmd_list = [ 120 | f"DISPLAY={os.getenv('DISPLAY')}", 121 | "TURTLEBOT3_MODEL=burger", 122 | "ros2", 123 | "launch", 124 | ros_pkg, 125 | sim_map, 126 | ] 127 | cmd = " ".join(cmd_list) 128 | # print("command:", cmd) 129 | 130 | pgrp = sp.Popen( 131 | cmd, 132 | shell=True, 133 | preexec_fn=os.setpgrp, 134 | stdout=sp.DEVNULL, 135 | stderr=sp.DEVNULL, 136 | ) 137 | return pgrp 138 | 139 | 140 | def run_tb3_hitl(tb3_uri): 141 | cmd = f"ssh -i keys/tb3 {tb3_uri} ./run.sh" 142 | 143 | proc = sp.Popen(cmd, shell=True, stdout=sp.DEVNULL, stderr=sp.DEVNULL) 144 | return proc 145 | 146 | 147 | def run_rcl_api_harness(features, targets, job): 148 | feature_str = " ".join(features) 149 | target_str = " ".join(targets) 150 | 151 | cmd = f"python3 rcl_harness.py -f {feature_str} -t {target_str} -j {job}" 152 | 153 | pgrp = sp.Popen( 154 | cmd, 155 | shell=True, 156 | preexec_fn=os.setpgrp, 157 | stdout=sys.stdout, 158 | stderr=sys.stderr, 159 | ) 160 | 161 | return pgrp 162 | 163 | 164 | def run_cli_harness(): 165 | cmd = "python3 cli_harness.py" 166 | 167 | pgrp = sp.Popen( 168 | cmd, 169 | shell=True, 170 | preexec_fn=os.setpgrp, 171 | stdout=sys.stdout, 172 | stderr=sys.stderr, 173 | ) 174 | 175 | return pgrp 176 | 177 | 178 | def run_rosidl_harness(lang, shmid, ros_type="empty"): 179 | # lang, shm, ros_type 180 | # need shm for both lang targets as they need to fetch data from shm 181 | 182 | # if lang == "py": 183 | # cmd = "" 184 | # elif lang == "cpp": 185 | # cmd = "" 186 | 187 | cmd = "ros2 run idltest_target idltest_target" 188 | 189 | pgrp = sp.Popen( 190 | cmd, 191 | shell=True, 192 | preexec_fn=os.setpgrp, 193 | stdout=sys.stdout, 194 | stderr=sys.stderr, 195 | ) 196 | 197 | return pgrp 198 | 199 | 200 | def run_moveit_harness(): 201 | cmd = f"DISPLAY={os.getenv('DISPLAY')} ros2 launch moveit2_tutorials move_group.launch.py 2>&1 > /dev/null" 202 | 203 | pgrp = sp.Popen( 204 | cmd, 205 | shell=True, 206 | preexec_fn=os.setpgrp, 207 | stdout=sys.stdout, 208 | stderr=sys.stderr, 209 | ) 210 | 211 | return pgrp 212 | 213 | 214 | def get_init_moveit_msg(): 215 | from moveit_msgs.msg import MotionPlanRequest 216 | 217 | # initial (ready) position 218 | with open("moveit_panda_msg.json", "r") as f: 219 | msg_json = json.load(f) 220 | 221 | msg = MotionPlanRequest() 222 | set_message_fields(msg, msg_json) 223 | 224 | # re-assign timestamp 225 | # ts = time.time_ns() 226 | # sec = int(ts / pow(10, 9)) 227 | # nsec = ts - sec * pow(10, 9) 228 | # msg.workspace_parameters.header.stamp.sec = sec 229 | # msg.workspace_parameters.header.stamp.nanosec = nsec 230 | 231 | return msg 232 | 233 | 234 | def get_init_joint_constraints(): 235 | msg = get_init_moveit_msg() 236 | joint_constraints = msg["goal_constraints"][0]["joint_constraints"] 237 | return joint_constraints 238 | 239 | 240 | def get_init_moveit_pose(): 241 | from geometry_msgs.msg import Pose 242 | msg = Pose() 243 | # msg.position.x = 0.28 244 | # msg.position.y = -0.2 245 | # msg.position.z = 0.5 246 | # msg.orientation.w = 1.0 247 | msg.position.x = 0.5 248 | msg.position.y = 0.5 249 | msg.position.z = 0.5 250 | msg.orientation.w = 1.0 251 | 252 | return msg 253 | 254 | def moveit_send_command(msg): 255 | print("[moveit harness] sending goal command") 256 | x = str(msg.position.x) 257 | y = str(msg.position.y) 258 | z = str(msg.position.z) 259 | w = str(msg.orientation.w) 260 | 261 | cmd = "ros2 launch moveit2_tutorials move_group_interface_tutorial.launch.py" 262 | 263 | sp.call( 264 | [ 265 | "ros2", 266 | "launch", 267 | "moveit2_tutorials", 268 | "move_group_interface_tutorial.launch.py", 269 | f"x:={x}", 270 | f"y:={y}", 271 | f"z:={z}", 272 | f"w:={w}", 273 | ], 274 | stdout=sp.DEVNULL, 275 | stderr=sp.DEVNULL, 276 | ) 277 | print(" + sent") 278 | 279 | 280 | if __name__ == "__main__": 281 | # run_px4_stack_sh() 282 | # pgrp = run_tb3_sitl(os.path.join(os.getcwd(), "../")) 283 | pgrp = run_moveit2_harness() 284 | print("pgroup pid:", pgrp.pid) 285 | time.sleep(20) 286 | try: 287 | os.killpg(pgrp.pid, signal.SIGKILL) 288 | except: 289 | print("err") 290 | -------------------------------------------------------------------------------- /src/idltest_replayer.py: -------------------------------------------------------------------------------- 1 | import os 2 | import pickle 3 | import time 4 | 5 | import rclpy 6 | import ros_utils 7 | from rosidl_runtime_py import message_to_ordereddict, set_message_fields 8 | 9 | import config 10 | from fuzzer import Fuzzer 11 | from executor import Executor, ExecMode 12 | from rosbag_parser import RosbagParser 13 | import checker 14 | 15 | def retrieve_message(frame, directory): 16 | 17 | found = False 18 | p_msg = "" 19 | for p_msg in os.listdir(os.path.join(directory, "queue")): 20 | if frame in p_msg: 21 | found = True 22 | break 23 | 24 | if not found: 25 | print("couldn't find msg") 26 | exit(-1) 27 | 28 | f = open(os.path.join(directory, "queue", p_msg), "rb") 29 | msg_dict = pickle.load(f) 30 | f.close() 31 | 32 | # print(msg_dict["data"]) 33 | 34 | for meta in os.listdir(os.path.join(directory, "metadata")): 35 | if frame in meta: 36 | found = True 37 | break 38 | 39 | f = open(os.path.join(directory, "metadata", meta), "rb") 40 | meta_dump = f.readline() 41 | f.close() 42 | 43 | meta = meta_dump.split(b"\t") 44 | class_str = meta[2].decode("utf-8") 45 | 46 | # regen class 47 | type_name = class_str.split("'")[1].split(".")[-1] 48 | # type_name = "BoolUnboundedDynArray" # overriding it for now 49 | 50 | msg_type_class = ros_utils.get_msg_class_from_name( 51 | "idltest_msgs", type_name) 52 | msg = msg_type_class() 53 | 54 | if "Array" in type_name: 55 | for d in msg_dict["data"]: 56 | msg.data.append(d) 57 | else: 58 | msg.data = msg_dict["data"] 59 | 60 | print(msg) 61 | 62 | return (msg, msg_type_class, type_name) 63 | 64 | if __name__ == "__main__": 65 | rclpy.init() 66 | 67 | frame = "1636629431.2263825" 68 | directory = "../logs/20211110-163031/" 69 | 70 | (msg, msg_type_class, type_name) = retrieve_message(frame, directory) 71 | topic_name = f"/idltest_{type_name}_in" 72 | msg_list = [msg] 73 | 74 | node = rclpy.create_node("replay_pub") 75 | pub = node.create_publisher(msg_type_class, topic_name, 10) 76 | print("publishing") 77 | pub.publish(msg) 78 | time.sleep(1) 79 | 80 | if False: 81 | config = config.RuntimeConfig() 82 | config.test_rosidl = True 83 | config.test_rosidl_lang = "cpp" 84 | config.watchlist = "watchlist/idltest.json" 85 | config.replay = True 86 | 87 | fuzzer = Fuzzer("_fuzzer", config) 88 | 89 | executor = Executor(fuzzer) 90 | executor.prep_execution(msg_type_class, topic_name) 91 | fuzzer.executor = executor 92 | 93 | pre_exec_list = None 94 | post_exec_list = None 95 | pub_function = None 96 | 97 | (retval, failure_msg) = executor.execute( 98 | mode=ExecMode.SINGLE, 99 | msg_list=msg_list, 100 | frame=frame, 101 | frequency=1, 102 | repeat=1, 103 | pre_exec_list=pre_exec_list, 104 | post_exec_list=post_exec_list, 105 | pub_function=pub_function, 106 | wait_lock=None, 107 | ) 108 | executor.clear_execution() 109 | 110 | if retval: 111 | print("publish failed:", failure_msg) 112 | else: 113 | exec_cnt = 0 114 | parser = RosbagParser( 115 | f"states-{exec_cnt}.bag/states-{exec_cnt}.bag_0.db3" 116 | ) 117 | 118 | state_msgs_dict = parser.process_messages() 119 | print("-----") 120 | print(state_msgs_dict) 121 | print("=====") 122 | 123 | errs = checker.run_checks(fuzzer.config, msg_list, state_msgs_dict) 124 | errs = list(set(errs)) 125 | 126 | fuzzer.kill_target() 127 | fuzzer.destroy() 128 | 129 | rclpy.shutdown() 130 | -------------------------------------------------------------------------------- /src/inspector.py: -------------------------------------------------------------------------------- 1 | def inspect_target(fuzzer): 2 | fuzz_targets = [] 3 | 4 | built_in_msg_types = ros_utils.get_all_message_types() 5 | subscriptions = ros_utils.get_subscriptions(fuzzer.node_ptr) 6 | 7 | if fuzzer.config.px4_sitl: 8 | if fuzzer.config.use_mavlink: 9 | topic_name = "/dummy_mavlink_topic" 10 | msg_type_class = ros_utils.get_msg_class_from_name( 11 | "px4_msgs", "ManualControlSetpoint" 12 | ) 13 | else: 14 | topic_name = "/TrajectorySetpoint_PubSubTopic" 15 | msg_type_class = ros_utils.get_msg_class_from_name( 16 | "px4_msgs", "TrajectorySetpoint" 17 | ) 18 | 19 | fuzz_targets.append([topic_name, msg_type_class, "drone"]) 20 | return fuzz_targets 21 | 22 | elif fuzzer.config.tb3_sitl: 23 | topic_name = "/cmd_vel" 24 | msg_type_class = ros_utils.get_msg_class_from_name( 25 | "geometry_msgs", "Twist" 26 | ) 27 | fuzz_targets.append( 28 | [topic_name, msg_type_class, "/turtlebot3_diff_drive"] 29 | ) 30 | 31 | return fuzz_targets 32 | 33 | elif fuzzer.config.tb3_hitl: 34 | topic_name = "/cmd_vel" 35 | msg_type_class = ros_utils.get_msg_class_from_name( 36 | "geometry_msgs", "Twist" 37 | ) 38 | fuzz_targets.append([topic_name, msg_type_class, "/turtlebot3_node"]) 39 | 40 | return fuzz_targets 41 | 42 | elif fuzzer.config.test_rosidl: 43 | topic_name = "/metatopic" # fake topic (topic tbd by mutator) 44 | msg_type_class = ros_utils.get_msg_class_from_name( 45 | "std_msgs", "Empty" 46 | ) 47 | fuzz_targets.append([topic_name, msg_type_class, "/rosidl_node"]) 48 | 49 | return fuzz_targets 50 | 51 | elif fuzzer.config.test_moveit: 52 | # use below if testing without commander harness 53 | topic_name = "/motion_plan_request" 54 | msg_type_class = ros_utils.get_msg_class_from_name( 55 | "moveit_msgs", "MotionPlanRequest" 56 | ) 57 | # for now, use commander harness for convenience 58 | topic_name = "/metatopic" 59 | msg_type_class = ros_utils.get_msg_class_from_name( 60 | "geometry_msgs", "Pose" 61 | ) 62 | 63 | fuzz_targets.append([topic_name, msg_type_class, "/moveit_node"]) 64 | 65 | return fuzz_targets 66 | 67 | for subscriber_node in subscriptions: 68 | print() 69 | print("[+] processing", subscriber_node) 70 | 71 | if subscriber_node.replace("/", "").startswith("_"): 72 | print("[-] skip internal node") 73 | continue 74 | 75 | for ti, topic in enumerate(subscriptions[subscriber_node]): 76 | topic_name = topic[0] 77 | msg_type_full = topic[1] 78 | 79 | print("[{}] {} {}".format(ti, topic_name, msg_type_full)) 80 | 81 | if len(msg_type_full) > 1: 82 | print("[check] MULTIPLE MESSAGE TYPES!") 83 | 84 | msg_type = msg_type_full[0] 85 | msg_pkg = msg_type.split("/")[0] 86 | msg_name = msg_type.split("/")[-1] 87 | 88 | if msg_name == "ParameterEvent": 89 | print("[-] skip ParameterEvents") 90 | continue 91 | 92 | if msg_pkg in built_in_msg_types.keys(): 93 | msg_type_class = ros_utils.get_msg_class_from_name( 94 | msg_pkg, msg_name 95 | ) 96 | else: 97 | msg_type_class = ros_utils.find_custom_msg(msg_type) 98 | 99 | if msg_type_class is None: 100 | print("[-] couldn't find msg class") 101 | continue 102 | 103 | print() 104 | print("Found fuzzing topic", topic_name, "of type", msg_type_class) 105 | print("- target node:", subscriber_node) 106 | fuzz_targets.append([topic_name, msg_type_class, subscriber_node]) 107 | 108 | return fuzz_targets 109 | 110 | 111 | def inspect_secure_target(fuzzer): 112 | fuzz_targets = [] 113 | 114 | built_in_msg_types = ros_utils.get_all_message_types() 115 | subscriptions = ros_utils.get_secure_subscriptions(fuzzer.node_ptr) 116 | 117 | for subscriber_node in subscriptions: 118 | print() 119 | print("[+] processing", subscriber_node) 120 | 121 | if subscriber_node.replace("/", "").startswith("_"): 122 | print("[-] skip internal node") 123 | continue 124 | 125 | for ti, topic in enumerate(subscriptions[subscriber_node]): 126 | topic_name = topic[0] 127 | msg_type_full = topic[1] 128 | 129 | print("[{}] {} {}".format(ti, topic_name, msg_type_full)) 130 | 131 | if len(msg_type_full) > 1: 132 | print("[check] MULTIPLE MESSAGE TYPES!") 133 | 134 | msg_type = msg_type_full[0] 135 | msg_pkg = msg_type.split("/")[0] 136 | msg_name = msg_type.split("/")[-1] 137 | 138 | if msg_name == "ParameterEvent": 139 | print("[-] skip ParameterEvents") 140 | continue 141 | 142 | if msg_pkg in built_in_msg_types.keys(): 143 | msg_type_class = ros_utils.get_msg_class_from_name( 144 | msg_pkg, msg_name 145 | ) 146 | else: 147 | msg_type_class = ros_utils.find_custom_msg(msg_type) 148 | 149 | if msg_type_class is None: 150 | print("[-] couldn't find msg class") 151 | continue 152 | 153 | print() 154 | print("Found fuzzing topic", topic_name, "of type", msg_type_class) 155 | print("- target node:", subscriber_node) 156 | fuzz_targets.append([topic_name, msg_type_class, subscriber_node]) 157 | 158 | fuzzer.kill_target() 159 | return fuzz_targets 160 | 161 | def inspect_service(fuzzer): 162 | services = ros_utils.get_services(fuzzer.node_ptr) 163 | built_in_svc_types = ros_utils.get_all_service_types() 164 | 165 | target_services = [] 166 | for tested_node in services: 167 | print() 168 | print("[+] processing {}'s services".format(tested_node)) 169 | for si, srv in enumerate(services[tested_node]): 170 | srv_name = srv[0] 171 | srv_type_full = srv[1] 172 | print("[{}] {} {}".format(si, srv_name, srv_type_full)) 173 | 174 | if len(srv_type_full) > 1: 175 | print("[check] MULTIPLE SERVICE TYPES!") 176 | 177 | if "parameter" in srv_name: 178 | print("[-] skip param servers") 179 | continue 180 | 181 | srv_type_full = srv_type_full[0] 182 | ss = srv_type_full.split("/") 183 | if len(ss) == 2: 184 | ss = [ss[0], "srv", ss[1]] 185 | srv_pkg = ss[0] 186 | srv_type = ss[-1] 187 | if srv_pkg == "action_msgs": 188 | print("[-] skip action messages") 189 | continue 190 | 191 | if ss[1] != "srv": 192 | print("[-] skip non-svc services") 193 | continue 194 | 195 | if srv_pkg not in built_in_svc_types.keys(): 196 | print("service package not found") 197 | continue 198 | 199 | srv_module_name = ".".join(ss[:-1]) 200 | try: 201 | module = importlib.import_module(srv_module_name) 202 | srv_module = getattr(module, srv_type) 203 | if not srv_pkg or not srv_module: 204 | raise ValueError() 205 | except ValueError: 206 | raise RuntimeError("service type is invalid") 207 | 208 | target_services.append([srv_name, srv_module, srv_type_full]) 209 | 210 | return target_services 211 | -------------------------------------------------------------------------------- /src/librcl_apis/client.txt: -------------------------------------------------------------------------------- 1 | rcl_get_zero_initialized_client 2 | rcl_client_init 3 | rcl_client_fini 4 | rcl_client_get_default_options 5 | rcl_send_request 6 | rcl_take_response_with_info 7 | rcl_take_response 8 | rcl_client_get_service_name 9 | rcl_client_get_options 10 | rcl_client_get_rmw_handle 11 | rcl_client_is_valid 12 | -------------------------------------------------------------------------------- /src/librcl_apis/expand_topic_name.txt: -------------------------------------------------------------------------------- 1 | rcl_expand_topic_name 2 | rcl_get_default_topic_name_substitutions 3 | -------------------------------------------------------------------------------- /src/librcl_apis/format.py: -------------------------------------------------------------------------------- 1 | s = """\ 2 | rcl_get_zero_initialized_publisher 3 | rcl_publisher_init 4 | rcl_publisher_fini 5 | rcl_publisher_get_default_options 6 | rcl_borrow_loaned_message 7 | rcl_return_loaned_message_from_publisher 8 | rcl_publish 9 | rcl_publish_serialized_message 10 | rcl_publish_loaned_message 11 | rcl_publisher_assert_liveliness 12 | rcl_publisher_get_topic_name 13 | rcl_publisher_get_options 14 | rcl_publisher_get_rmw_handle 15 | rcl_publisher_get_context 16 | rcl_publisher_is_valid 17 | rcl_publisher_is_valid_except_context 18 | rcl_publisher_get_subscription_count 19 | rcl_publisher_get_actual_qos 20 | rcl_publisher_can_loan_messages""" 21 | 22 | 23 | ss = "" 24 | for a in s.split("\n"): 25 | ss += " -x " + a + "@librcl.so" 26 | 27 | print(ss) 28 | 29 | -------------------------------------------------------------------------------- /src/librcl_apis/graph.txt: -------------------------------------------------------------------------------- 1 | rcl_get_publisher_names_and_types_by_node 2 | rcl_get_subscriber_names_and_types_by_node 3 | rcl_get_service_names_and_types_by_node 4 | rcl_get_client_names_and_types_by_node 5 | rcl_get_topic_names_and_types 6 | rcl_get_service_names_and_types 7 | rcl_names_and_types_init 8 | rcl_names_and_types_fini 9 | rcl_get_node_names 10 | rcl_get_node_names_with_enclaves 11 | rcl_count_publishers 12 | rcl_count_subscribers 13 | rcl_get_publishers_info_by_topic 14 | rcl_get_subscriptions_info_by_topic 15 | rcl_service_server_is_available 16 | -------------------------------------------------------------------------------- /src/librcl_apis/guard_condition.txt: -------------------------------------------------------------------------------- 1 | rcl_get_zero_initialized_guard_condition 2 | rcl_guard_condition_init 3 | rcl_guard_condition_init_from_rmw 4 | rcl_guard_condition_fini 5 | rcl_guard_condition_get_default_options 6 | rcl_trigger_guard_condition 7 | rcl_guard_condition_get_options 8 | rcl_guard_condition_get_rmw_handle 9 | -------------------------------------------------------------------------------- /src/librcl_apis/init.txt: -------------------------------------------------------------------------------- 1 | rcl_init 2 | rcl_shutdown 3 | -------------------------------------------------------------------------------- /src/librcl_apis/node.txt: -------------------------------------------------------------------------------- 1 | rcl_get_zero_initialized_node 2 | rcl_node_init 3 | rcl_node_fini 4 | rcl_node_is_valid 5 | rcl_node_is_valid_except_context 6 | rcl_node_get_name 7 | rcl_node_get_namespace 8 | rcl_node_get_fully_qualified_name 9 | rcl_node_get_options 10 | rcl_node_get_domain_id 11 | rcl_node_get_rmw_handle 12 | rcl_node_get_rcl_instance_id 13 | rcl_node_get_graph_guard_condition 14 | rcl_node_get_logger_name 15 | -------------------------------------------------------------------------------- /src/librcl_apis/publisher.txt: -------------------------------------------------------------------------------- 1 | rcl_get_zero_initialized_publisher 2 | rcl_publisher_init 3 | rcl_publisher_fini 4 | rcl_publisher_get_default_options 5 | rcl_borrow_loaned_message 6 | rcl_return_loaned_message_from_publisher 7 | rcl_publish 8 | rcl_publish_serialized_message 9 | rcl_publish_loaned_message 10 | rcl_publisher_assert_liveliness 11 | rcl_publisher_get_topic_name 12 | rcl_publisher_get_options 13 | rcl_publisher_get_rmw_handle 14 | rcl_publisher_get_context 15 | rcl_publisher_is_valid 16 | rcl_publisher_is_valid_except_context 17 | rcl_publisher_get_subscription_count 18 | rcl_publisher_get_actual_qos 19 | rcl_publisher_can_loan_messages 20 | -------------------------------------------------------------------------------- /src/librcl_apis/service.txt: -------------------------------------------------------------------------------- 1 | rcl_get_zero_initialized_service 2 | rcl_service_init 3 | rcl_service_fini 4 | rcl_service_get_default_options 5 | rcl_take_request_with_info 6 | rcl_take_request 7 | rcl_send_response 8 | rcl_service_get_service_name 9 | rcl_service_get_options 10 | rcl_service_get_rmw_handle 11 | rcl_service_is_valid 12 | -------------------------------------------------------------------------------- /src/librcl_apis/subscriber.txt: -------------------------------------------------------------------------------- 1 | rcl_get_zero_initialized_subscription 2 | rcl_subscription_init 3 | rcl_subscription_fini 4 | rcl_subscription_get_default_options 5 | rcl_take 6 | rcl_take_sequence 7 | rcl_take_serialized_message 8 | rcl_take_loaned_message 9 | rcl_return_loaned_message_from_subscription 10 | rcl_subscription_get_topic_name 11 | rcl_subscription_get_options 12 | rcl_subscription_get_rmw_handle 13 | rcl_subscription_is_valid 14 | rcl_subscription_get_publisher_count 15 | rcl_subscription_get_actual_qos 16 | rcl_subscription_can_loan_messages 17 | -------------------------------------------------------------------------------- /src/librcl_apis/time.txt: -------------------------------------------------------------------------------- 1 | rcl_clock_valid 2 | rcl_clock_init 3 | rcl_clock_fini 4 | rcl_ros_clock_init 5 | rcl_ros_clock_fini 6 | rcl_steady_clock_init 7 | rcl_steady_clock_fini 8 | rcl_system_clock_init 9 | rcl_system_clock_fini 10 | rcl_difference_times 11 | rcl_clock_get_now 12 | rcl_enable_ros_time_override 13 | rcl_disable_ros_time_override 14 | rcl_is_enabled_ros_time_override 15 | rcl_set_ros_time_override 16 | rcl_clock_add_jump_callback 17 | rcl_clock_remove_jump_callback 18 | -------------------------------------------------------------------------------- /src/librcl_apis/timer.txt: -------------------------------------------------------------------------------- 1 | rcl_get_zero_initialized_timer 2 | rcl_timer_init 3 | rcl_timer_fini 4 | rcl_timer_call 5 | rcl_timer_clock 6 | rcl_timer_is_ready 7 | rcl_timer_get_time_until_next_call 8 | rcl_timer_get_time_since_last_call 9 | rcl_timer_get_period 10 | rcl_timer_exchange_period 11 | rcl_timer_get_callback 12 | rcl_timer_exchange_callback 13 | rcl_timer_cancel 14 | rcl_timer_is_canceled 15 | rcl_timer_reset 16 | rcl_timer_get_allocator 17 | rcl_timer_get_guard_condition 18 | -------------------------------------------------------------------------------- /src/librcl_apis/validate_topic_name.txt: -------------------------------------------------------------------------------- 1 | rcl_validate_topic_name 2 | rcl_validate_topic_name_with_size 3 | rcl_topic_name_validation_result_string 4 | -------------------------------------------------------------------------------- /src/librcl_apis/wait.txt: -------------------------------------------------------------------------------- 1 | rcl_get_zero_initialized_wait_set 2 | rcl_wait_set_init 3 | rcl_wait_set_fini 4 | rcl_wait_set_get_allocator 5 | rcl_wait_set_add_subscription 6 | rcl_wait_set_clear 7 | rcl_wait_set_resize 8 | rcl_wait_set_add_guard_condition 9 | rcl_wait_set_add_timer 10 | rcl_wait_set_add_client 11 | rcl_wait_set_add_service 12 | rcl_wait_set_add_event 13 | rcl_wait 14 | rcl_wait_set_is_valid 15 | -------------------------------------------------------------------------------- /src/missions/mission_generator.py: -------------------------------------------------------------------------------- 1 | import json 2 | 3 | from rosidl_runtime_py import message_to_ordereddict, set_message_fields 4 | from px4_msgs.msg import TrajectorySetpoint 5 | 6 | 7 | # missionfile = open("church-fail.json", "w") 8 | missionfile = open("takeoff-quickland.json", "w") 9 | 10 | msg_list = [] 11 | 12 | # takeoff 13 | msg = TrajectorySetpoint() 14 | msg.z = -10.0 15 | msg.yaw = 0.0 16 | # msg.acceleration[2] = -3000.0 17 | 18 | md = message_to_ordereddict(msg) 19 | for i in range(50): 20 | msg_list.append(md) 21 | 22 | # quick landing 23 | msg = TrajectorySetpoint() 24 | msg.z = 0.0 25 | msg.yaw = 0.0 26 | msg.acceleration[2] = 3000.0 27 | 28 | md = message_to_ordereddict(msg) 29 | for i in range(50): 30 | msg_list.append(md) 31 | 32 | # msg = TrajectorySetpoint() 33 | # msg.x = 20.0 34 | # msg.y = 10.0 35 | # msg.z = -5.0 36 | # msg.yaw = 1.0 37 | # msg.acceleration[1] = 3000.0 38 | 39 | # md_f = message_to_ordereddict(msg) 40 | 41 | # # waypoint 1 42 | # msg = TrajectorySetpoint() 43 | # msg.x = 20.0 44 | # msg.y = 10.0 45 | # msg.z = -5.0 46 | # msg.yaw = 1.0 47 | 48 | # md = message_to_ordereddict(msg) 49 | # for i in range(50): 50 | # msg_list.append(md) 51 | 52 | # # inject failure 53 | # # if i == 20: 54 | # # msg_list.append(md_f) 55 | 56 | # # waypoint 2 57 | # msg = TrajectorySetpoint() 58 | # msg.x = 35.0 59 | # msg.y = 10.0 60 | # msg.z = -5.0 61 | # msg.yaw = -0.2 62 | 63 | # md = message_to_ordereddict(msg) 64 | # for i in range(50): 65 | # msg_list.append(md) 66 | 67 | # # waypoint 3 68 | # msg = TrajectorySetpoint() 69 | # msg.x = 25.0 70 | # msg.y = -10.0 71 | # msg.z = -5.0 72 | # msg.yaw = -1.8 73 | 74 | # md = message_to_ordereddict(msg) 75 | # for i in range(50): 76 | # msg_list.append(md) 77 | 78 | # # waypoint 4 (return) 79 | # msg = TrajectorySetpoint() 80 | # msg.x = 0.0 81 | # msg.y = 0.0 82 | # msg.z = -5.0 83 | # msg.yaw = 3.0 84 | 85 | # md = message_to_ordereddict(msg) 86 | # for i in range(50): 87 | # msg_list.append(md) 88 | 89 | # # landing 90 | # msg = TrajectorySetpoint() 91 | # msg.x = 0.0 92 | # msg.y = 0.0 93 | # msg.z = -0.0 94 | # msg.yaw = 0.0 95 | 96 | # md = message_to_ordereddict(msg) 97 | # for i in range(100): 98 | # msg_list.append(md) 99 | 100 | json.dump(msg_list, missionfile) 101 | missionfile.close() 102 | -------------------------------------------------------------------------------- /src/missions/mission_generator2.py: -------------------------------------------------------------------------------- 1 | import json 2 | 3 | from rosidl_runtime_py import message_to_ordereddict, set_message_fields 4 | from px4_msgs.msg import TrajectorySetpoint 5 | 6 | # missionfile = open("church-fail.json", "w") 7 | missionfile = open("takeoff-push.json", "w") 8 | 9 | msg_list = [] 10 | 11 | # takeoff 12 | msg = TrajectorySetpoint() 13 | msg.z = -10.0 14 | msg.yaw = 0.0 15 | msg.acceleration[2] = -3000.0 16 | 17 | md = message_to_ordereddict(msg) 18 | for i in range(50): 19 | msg_list.append(md) 20 | 21 | json.dump(msg_list, missionfile) 22 | missionfile.close() 23 | -------------------------------------------------------------------------------- /src/missions/mission_generator3.py: -------------------------------------------------------------------------------- 1 | import json 2 | 3 | from rosidl_runtime_py import message_to_ordereddict, set_message_fields 4 | from px4_msgs.msg import TrajectorySetpoint 5 | 6 | missionfile = open("takeoff-pushdown.json", "w") 7 | 8 | msg_list = [] 9 | 10 | # normal takeoff to z = 20 11 | msg = TrajectorySetpoint() 12 | msg.z = -30.0 13 | msg.yaw = 0.0 14 | # msg.acceleration[2] = -3000.0 15 | 16 | md = message_to_ordereddict(msg) 17 | for i in range(100): 18 | msg_list.append(md) 19 | 20 | # pushdown to z = 10 21 | msg = TrajectorySetpoint() 22 | msg.z = 0.0 23 | msg.yaw = 0.0 24 | msg.acceleration[2] = 5000.0 25 | 26 | md = message_to_ordereddict(msg) 27 | for i in range(5): 28 | msg_list.append(md) 29 | 30 | json.dump(msg_list, missionfile) 31 | missionfile.close() 32 | -------------------------------------------------------------------------------- /src/missions/takeoff-push.json: -------------------------------------------------------------------------------- 1 | [{"timestamp": 0, "x": 0.0, "y": 0.0, "z": -10.0, "yaw": 0.0, "yawspeed": 0.0, "vx": 0.0, "vy": 0.0, "vz": 0.0, "acceleration": [0.0, 0.0, -3000.0], "jerk": [0.0, 0.0, 0.0], "thrust": [0.0, 0.0, 0.0]}, {"timestamp": 0, "x": 0.0, "y": 0.0, "z": -10.0, "yaw": 0.0, "yawspeed": 0.0, "vx": 0.0, "vy": 0.0, "vz": 0.0, "acceleration": [0.0, 0.0, -3000.0], "jerk": [0.0, 0.0, 0.0], "thrust": [0.0, 0.0, 0.0]}, {"timestamp": 0, "x": 0.0, "y": 0.0, "z": -10.0, "yaw": 0.0, "yawspeed": 0.0, "vx": 0.0, "vy": 0.0, "vz": 0.0, "acceleration": [0.0, 0.0, -3000.0], "jerk": [0.0, 0.0, 0.0], "thrust": [0.0, 0.0, 0.0]}, {"timestamp": 0, "x": 0.0, "y": 0.0, "z": -10.0, "yaw": 0.0, "yawspeed": 0.0, "vx": 0.0, "vy": 0.0, "vz": 0.0, "acceleration": [0.0, 0.0, -3000.0], "jerk": [0.0, 0.0, 0.0], "thrust": [0.0, 0.0, 0.0]}, {"timestamp": 0, "x": 0.0, "y": 0.0, "z": -10.0, "yaw": 0.0, "yawspeed": 0.0, "vx": 0.0, "vy": 0.0, "vz": 0.0, "acceleration": [0.0, 0.0, -3000.0], "jerk": [0.0, 0.0, 0.0], "thrust": [0.0, 0.0, 0.0]}, {"timestamp": 0, "x": 0.0, "y": 0.0, "z": -10.0, "yaw": 0.0, "yawspeed": 0.0, "vx": 0.0, "vy": 0.0, "vz": 0.0, "acceleration": [0.0, 0.0, -3000.0], "jerk": [0.0, 0.0, 0.0], "thrust": [0.0, 0.0, 0.0]}, {"timestamp": 0, "x": 0.0, "y": 0.0, "z": -10.0, "yaw": 0.0, "yawspeed": 0.0, "vx": 0.0, "vy": 0.0, "vz": 0.0, "acceleration": [0.0, 0.0, -3000.0], "jerk": [0.0, 0.0, 0.0], "thrust": [0.0, 0.0, 0.0]}, {"timestamp": 0, "x": 0.0, "y": 0.0, "z": -10.0, "yaw": 0.0, "yawspeed": 0.0, "vx": 0.0, "vy": 0.0, "vz": 0.0, "acceleration": [0.0, 0.0, -3000.0], "jerk": [0.0, 0.0, 0.0], "thrust": [0.0, 0.0, 0.0]}, {"timestamp": 0, "x": 0.0, "y": 0.0, "z": -10.0, "yaw": 0.0, "yawspeed": 0.0, "vx": 0.0, "vy": 0.0, "vz": 0.0, "acceleration": [0.0, 0.0, -3000.0], "jerk": [0.0, 0.0, 0.0], "thrust": [0.0, 0.0, 0.0]}, {"timestamp": 0, "x": 0.0, "y": 0.0, "z": -10.0, "yaw": 0.0, "yawspeed": 0.0, "vx": 0.0, "vy": 0.0, "vz": 0.0, "acceleration": [0.0, 0.0, -3000.0], "jerk": [0.0, 0.0, 0.0], "thrust": [0.0, 0.0, 0.0]}, {"timestamp": 0, "x": 0.0, "y": 0.0, "z": -10.0, "yaw": 0.0, "yawspeed": 0.0, "vx": 0.0, "vy": 0.0, "vz": 0.0, "acceleration": [0.0, 0.0, -3000.0], "jerk": [0.0, 0.0, 0.0], "thrust": [0.0, 0.0, 0.0]}, {"timestamp": 0, "x": 0.0, "y": 0.0, "z": -10.0, "yaw": 0.0, "yawspeed": 0.0, "vx": 0.0, "vy": 0.0, "vz": 0.0, "acceleration": [0.0, 0.0, -3000.0], "jerk": [0.0, 0.0, 0.0], "thrust": [0.0, 0.0, 0.0]}, {"timestamp": 0, "x": 0.0, "y": 0.0, "z": -10.0, "yaw": 0.0, "yawspeed": 0.0, "vx": 0.0, "vy": 0.0, "vz": 0.0, "acceleration": [0.0, 0.0, -3000.0], "jerk": [0.0, 0.0, 0.0], "thrust": [0.0, 0.0, 0.0]}, {"timestamp": 0, "x": 0.0, "y": 0.0, "z": -10.0, "yaw": 0.0, "yawspeed": 0.0, "vx": 0.0, "vy": 0.0, "vz": 0.0, "acceleration": [0.0, 0.0, -3000.0], "jerk": [0.0, 0.0, 0.0], "thrust": [0.0, 0.0, 0.0]}, {"timestamp": 0, "x": 0.0, "y": 0.0, "z": -10.0, "yaw": 0.0, "yawspeed": 0.0, "vx": 0.0, "vy": 0.0, "vz": 0.0, "acceleration": [0.0, 0.0, -3000.0], "jerk": [0.0, 0.0, 0.0], "thrust": [0.0, 0.0, 0.0]}, {"timestamp": 0, "x": 0.0, "y": 0.0, "z": -10.0, "yaw": 0.0, "yawspeed": 0.0, "vx": 0.0, "vy": 0.0, "vz": 0.0, "acceleration": [0.0, 0.0, -3000.0], "jerk": [0.0, 0.0, 0.0], "thrust": [0.0, 0.0, 0.0]}, {"timestamp": 0, "x": 0.0, "y": 0.0, "z": -10.0, "yaw": 0.0, "yawspeed": 0.0, "vx": 0.0, "vy": 0.0, "vz": 0.0, "acceleration": [0.0, 0.0, -3000.0], "jerk": [0.0, 0.0, 0.0], "thrust": [0.0, 0.0, 0.0]}, {"timestamp": 0, "x": 0.0, "y": 0.0, "z": -10.0, "yaw": 0.0, "yawspeed": 0.0, "vx": 0.0, "vy": 0.0, "vz": 0.0, "acceleration": [0.0, 0.0, -3000.0], "jerk": [0.0, 0.0, 0.0], "thrust": [0.0, 0.0, 0.0]}, {"timestamp": 0, "x": 0.0, "y": 0.0, "z": -10.0, "yaw": 0.0, "yawspeed": 0.0, "vx": 0.0, "vy": 0.0, "vz": 0.0, "acceleration": [0.0, 0.0, -3000.0], "jerk": [0.0, 0.0, 0.0], "thrust": [0.0, 0.0, 0.0]}, {"timestamp": 0, "x": 0.0, "y": 0.0, "z": -10.0, "yaw": 0.0, "yawspeed": 0.0, "vx": 0.0, "vy": 0.0, "vz": 0.0, "acceleration": [0.0, 0.0, -3000.0], "jerk": [0.0, 0.0, 0.0], "thrust": [0.0, 0.0, 0.0]}, {"timestamp": 0, "x": 0.0, "y": 0.0, "z": -10.0, "yaw": 0.0, "yawspeed": 0.0, "vx": 0.0, "vy": 0.0, "vz": 0.0, "acceleration": [0.0, 0.0, -3000.0], "jerk": [0.0, 0.0, 0.0], "thrust": [0.0, 0.0, 0.0]}, {"timestamp": 0, "x": 0.0, "y": 0.0, "z": -10.0, "yaw": 0.0, "yawspeed": 0.0, "vx": 0.0, "vy": 0.0, "vz": 0.0, "acceleration": [0.0, 0.0, -3000.0], "jerk": [0.0, 0.0, 0.0], "thrust": [0.0, 0.0, 0.0]}, {"timestamp": 0, "x": 0.0, "y": 0.0, "z": -10.0, "yaw": 0.0, "yawspeed": 0.0, "vx": 0.0, "vy": 0.0, "vz": 0.0, "acceleration": [0.0, 0.0, -3000.0], "jerk": [0.0, 0.0, 0.0], "thrust": [0.0, 0.0, 0.0]}, {"timestamp": 0, "x": 0.0, "y": 0.0, "z": -10.0, "yaw": 0.0, "yawspeed": 0.0, "vx": 0.0, "vy": 0.0, "vz": 0.0, "acceleration": [0.0, 0.0, -3000.0], "jerk": [0.0, 0.0, 0.0], "thrust": [0.0, 0.0, 0.0]}, {"timestamp": 0, "x": 0.0, "y": 0.0, "z": -10.0, "yaw": 0.0, "yawspeed": 0.0, "vx": 0.0, "vy": 0.0, "vz": 0.0, "acceleration": [0.0, 0.0, -3000.0], "jerk": [0.0, 0.0, 0.0], "thrust": [0.0, 0.0, 0.0]}, {"timestamp": 0, "x": 0.0, "y": 0.0, "z": -10.0, "yaw": 0.0, "yawspeed": 0.0, "vx": 0.0, "vy": 0.0, "vz": 0.0, "acceleration": [0.0, 0.0, -3000.0], "jerk": [0.0, 0.0, 0.0], "thrust": [0.0, 0.0, 0.0]}, {"timestamp": 0, "x": 0.0, "y": 0.0, "z": -10.0, "yaw": 0.0, "yawspeed": 0.0, "vx": 0.0, "vy": 0.0, "vz": 0.0, "acceleration": [0.0, 0.0, -3000.0], "jerk": [0.0, 0.0, 0.0], "thrust": [0.0, 0.0, 0.0]}, {"timestamp": 0, "x": 0.0, "y": 0.0, "z": -10.0, "yaw": 0.0, "yawspeed": 0.0, "vx": 0.0, "vy": 0.0, "vz": 0.0, "acceleration": [0.0, 0.0, -3000.0], "jerk": [0.0, 0.0, 0.0], "thrust": [0.0, 0.0, 0.0]}, {"timestamp": 0, "x": 0.0, "y": 0.0, "z": -10.0, "yaw": 0.0, "yawspeed": 0.0, "vx": 0.0, "vy": 0.0, "vz": 0.0, "acceleration": [0.0, 0.0, -3000.0], "jerk": [0.0, 0.0, 0.0], "thrust": [0.0, 0.0, 0.0]}, {"timestamp": 0, "x": 0.0, "y": 0.0, "z": -10.0, "yaw": 0.0, "yawspeed": 0.0, "vx": 0.0, "vy": 0.0, "vz": 0.0, "acceleration": [0.0, 0.0, -3000.0], "jerk": [0.0, 0.0, 0.0], "thrust": [0.0, 0.0, 0.0]}, {"timestamp": 0, "x": 0.0, "y": 0.0, "z": -10.0, "yaw": 0.0, "yawspeed": 0.0, "vx": 0.0, "vy": 0.0, "vz": 0.0, "acceleration": [0.0, 0.0, -3000.0], "jerk": [0.0, 0.0, 0.0], "thrust": [0.0, 0.0, 0.0]}, {"timestamp": 0, "x": 0.0, "y": 0.0, "z": -10.0, "yaw": 0.0, "yawspeed": 0.0, "vx": 0.0, "vy": 0.0, "vz": 0.0, "acceleration": [0.0, 0.0, -3000.0], "jerk": [0.0, 0.0, 0.0], "thrust": [0.0, 0.0, 0.0]}, {"timestamp": 0, "x": 0.0, "y": 0.0, "z": -10.0, "yaw": 0.0, "yawspeed": 0.0, "vx": 0.0, "vy": 0.0, "vz": 0.0, "acceleration": [0.0, 0.0, -3000.0], "jerk": [0.0, 0.0, 0.0], "thrust": [0.0, 0.0, 0.0]}, {"timestamp": 0, "x": 0.0, "y": 0.0, "z": -10.0, "yaw": 0.0, "yawspeed": 0.0, "vx": 0.0, "vy": 0.0, "vz": 0.0, "acceleration": [0.0, 0.0, -3000.0], "jerk": [0.0, 0.0, 0.0], "thrust": [0.0, 0.0, 0.0]}, {"timestamp": 0, "x": 0.0, "y": 0.0, "z": -10.0, "yaw": 0.0, "yawspeed": 0.0, "vx": 0.0, "vy": 0.0, "vz": 0.0, "acceleration": [0.0, 0.0, -3000.0], "jerk": [0.0, 0.0, 0.0], "thrust": [0.0, 0.0, 0.0]}, {"timestamp": 0, "x": 0.0, "y": 0.0, "z": -10.0, "yaw": 0.0, "yawspeed": 0.0, "vx": 0.0, "vy": 0.0, "vz": 0.0, "acceleration": [0.0, 0.0, -3000.0], "jerk": [0.0, 0.0, 0.0], "thrust": [0.0, 0.0, 0.0]}, {"timestamp": 0, "x": 0.0, "y": 0.0, "z": -10.0, "yaw": 0.0, "yawspeed": 0.0, "vx": 0.0, "vy": 0.0, "vz": 0.0, "acceleration": [0.0, 0.0, -3000.0], "jerk": [0.0, 0.0, 0.0], "thrust": [0.0, 0.0, 0.0]}, {"timestamp": 0, "x": 0.0, "y": 0.0, "z": -10.0, "yaw": 0.0, "yawspeed": 0.0, "vx": 0.0, "vy": 0.0, "vz": 0.0, "acceleration": [0.0, 0.0, -3000.0], "jerk": [0.0, 0.0, 0.0], "thrust": [0.0, 0.0, 0.0]}, {"timestamp": 0, "x": 0.0, "y": 0.0, "z": -10.0, "yaw": 0.0, "yawspeed": 0.0, "vx": 0.0, "vy": 0.0, "vz": 0.0, "acceleration": [0.0, 0.0, -3000.0], "jerk": [0.0, 0.0, 0.0], "thrust": [0.0, 0.0, 0.0]}, {"timestamp": 0, "x": 0.0, "y": 0.0, "z": -10.0, "yaw": 0.0, "yawspeed": 0.0, "vx": 0.0, "vy": 0.0, "vz": 0.0, "acceleration": [0.0, 0.0, -3000.0], "jerk": [0.0, 0.0, 0.0], "thrust": [0.0, 0.0, 0.0]}, {"timestamp": 0, "x": 0.0, "y": 0.0, "z": -10.0, "yaw": 0.0, "yawspeed": 0.0, "vx": 0.0, "vy": 0.0, "vz": 0.0, "acceleration": [0.0, 0.0, -3000.0], "jerk": [0.0, 0.0, 0.0], "thrust": [0.0, 0.0, 0.0]}, {"timestamp": 0, "x": 0.0, "y": 0.0, "z": -10.0, "yaw": 0.0, "yawspeed": 0.0, "vx": 0.0, "vy": 0.0, "vz": 0.0, "acceleration": [0.0, 0.0, -3000.0], "jerk": [0.0, 0.0, 0.0], "thrust": [0.0, 0.0, 0.0]}, {"timestamp": 0, "x": 0.0, "y": 0.0, "z": -10.0, "yaw": 0.0, "yawspeed": 0.0, "vx": 0.0, "vy": 0.0, "vz": 0.0, "acceleration": [0.0, 0.0, -3000.0], "jerk": [0.0, 0.0, 0.0], "thrust": [0.0, 0.0, 0.0]}, {"timestamp": 0, "x": 0.0, "y": 0.0, "z": -10.0, "yaw": 0.0, "yawspeed": 0.0, "vx": 0.0, "vy": 0.0, "vz": 0.0, "acceleration": [0.0, 0.0, -3000.0], "jerk": [0.0, 0.0, 0.0], "thrust": [0.0, 0.0, 0.0]}, {"timestamp": 0, "x": 0.0, "y": 0.0, "z": -10.0, "yaw": 0.0, "yawspeed": 0.0, "vx": 0.0, "vy": 0.0, "vz": 0.0, "acceleration": [0.0, 0.0, -3000.0], "jerk": [0.0, 0.0, 0.0], "thrust": [0.0, 0.0, 0.0]}, {"timestamp": 0, "x": 0.0, "y": 0.0, "z": -10.0, "yaw": 0.0, "yawspeed": 0.0, "vx": 0.0, "vy": 0.0, "vz": 0.0, "acceleration": [0.0, 0.0, -3000.0], "jerk": [0.0, 0.0, 0.0], "thrust": [0.0, 0.0, 0.0]}, {"timestamp": 0, "x": 0.0, "y": 0.0, "z": -10.0, "yaw": 0.0, "yawspeed": 0.0, "vx": 0.0, "vy": 0.0, "vz": 0.0, "acceleration": [0.0, 0.0, -3000.0], "jerk": [0.0, 0.0, 0.0], "thrust": [0.0, 0.0, 0.0]}, {"timestamp": 0, "x": 0.0, "y": 0.0, "z": -10.0, "yaw": 0.0, "yawspeed": 0.0, "vx": 0.0, "vy": 0.0, "vz": 0.0, "acceleration": [0.0, 0.0, -3000.0], "jerk": [0.0, 0.0, 0.0], "thrust": [0.0, 0.0, 0.0]}, {"timestamp": 0, "x": 0.0, "y": 0.0, "z": -10.0, "yaw": 0.0, "yawspeed": 0.0, "vx": 0.0, "vy": 0.0, "vz": 0.0, "acceleration": [0.0, 0.0, -3000.0], "jerk": [0.0, 0.0, 0.0], "thrust": [0.0, 0.0, 0.0]}, {"timestamp": 0, "x": 0.0, "y": 0.0, "z": -10.0, "yaw": 0.0, "yawspeed": 0.0, "vx": 0.0, "vy": 0.0, "vz": 0.0, "acceleration": [0.0, 0.0, -3000.0], "jerk": [0.0, 0.0, 0.0], "thrust": [0.0, 0.0, 0.0]}] -------------------------------------------------------------------------------- /src/oracles/px4.py: -------------------------------------------------------------------------------- 1 | import math 2 | import statistics 3 | 4 | 5 | def check(config, msg_list, state_dict, feedback_list): 6 | errs = list() 7 | 8 | try: 9 | sensor_accel_list = state_dict["/SensorAccel_PubSubTopic"] 10 | except KeyError: 11 | print("[checker] no SensorAccel data available") 12 | sensor_accel_list = list() 13 | 14 | try: 15 | sensor_baro_list = state_dict["/SensorBaro_PubSubTopic"] 16 | except KeyError: 17 | print("[checker] no SensorBaro data available") 18 | sensor_baro_list = list() 19 | 20 | try: 21 | sensor_gps_list = state_dict["/SensorGps_PubSubTopic"] 22 | except KeyError: 23 | print("[checker] no SensorGps data available") 24 | sensor_gps_list = list() 25 | 26 | try: 27 | sensor_gyro_list = state_dict["/SensorGyro_PubSubTopic"] 28 | except KeyError: 29 | print("[checker] no SensorGyro data available") 30 | sensor_gyro_list = list() 31 | 32 | try: 33 | sensor_imu_list = state_dict["/SensorsStatusImu_PubSubTopic"] 34 | except KeyError: 35 | print("[checker] no SensorsStatusImu data available") 36 | sensor_imu_list = list() 37 | 38 | try: 39 | sensor_combined_list = state_dict["/SensorCombined_PubSubTopic"] 40 | except KeyError: 41 | print("[checker] no SensorCombined data available") 42 | sensor_combined_list = list() 43 | 44 | try: 45 | vehicle_odometry_list = state_dict["/VehicleOdometry_PubSubTopic"] 46 | except KeyError: 47 | print("[checker] no SensorCombined data available") 48 | vehicle_odometry_list = list() 49 | 50 | try: 51 | vehicle_imu_list = state_dict["/VehicleImu_PubSubTopic"] 52 | except KeyError: 53 | print("[checker] no VehicleImu data available") 54 | vehicle_imu_list = list() 55 | 56 | try: 57 | vehicle_gps_list = state_dict["/VehicleGpsPosition_PubSubTopic"] 58 | except KeyError: 59 | print("[checker] no VehicleGpsPosition data available") 60 | vehicle_gps_list = list() 61 | 62 | try: 63 | vehicle_global_position_list = state_dict["/VehicleGlobalPosition_PubSubTopic"] 64 | except KeyError: 65 | print("[checker] no VehicleGlobalPosition data available") 66 | vehicle_global_position_list = list() 67 | 68 | try: 69 | vehicle_local_position_list = state_dict["/VehicleLocalPosition_PubSubTopic"] 70 | except KeyError: 71 | print("[checker] no VehicleLocalPosition data available") 72 | vehicle_local_position_list = list() 73 | 74 | try: 75 | vehicle_attitude_list = state_dict["/VehicleAttitude_PubSubTopic"] 76 | except KeyError: 77 | print("[checker] no VehicleAttitude data available") 78 | vehicle_attitude_list = list() 79 | 80 | try: 81 | vehicle_acceleration_list = state_dict["/VehicleAcceleration_PubSubTopic"] 82 | except KeyError: 83 | print("[checker] no VehicleAcceleration data available") 84 | vehicle_acceleration_list = list() 85 | 86 | try: 87 | vehicle_angular_velocity = state_dict["/VehicleAngularVelocity_PubSubTopic"] 88 | except KeyError: 89 | print("[checker] no VehicleAngularVelocity data available") 90 | vehicle_angular_velocity = list() 91 | 92 | try: 93 | vehicle_angular_acceleration = state_dict["/VehicleAngularAcceleration_PubSubTopic"] 94 | except KeyError: 95 | print("[checker] no VehicleAngularAcceleration data available") 96 | vehicle_angular_acceleration = list() 97 | 98 | # 0. Filter timestamps when the drone is on the ground or hit the 99 | # ground. Timestamps are in nanoseconds. 100 | filter_ts_list = list() 101 | local_x_list = list() 102 | local_y_list = list() 103 | local_z_list = list() 104 | for (ts, pos) in vehicle_local_position_list: 105 | dist_bottom = pos.dist_bottom 106 | 107 | if dist_bottom < 0.15: # unit: meters 108 | filter_ts_list.append(ts) 109 | 110 | # to use in step 3 111 | local_x_list.append(pos.x) 112 | local_y_list.append(pos.y) 113 | local_z_list.append(pos.z) 114 | 115 | # 1. check for parameter violation 116 | # http://docs.px4.io/master/en/advanced_config/parameter_reference.html 117 | 118 | # 1.1. Acceleration 119 | for (ts, acc) in vehicle_acceleration_list: 120 | acc_x = acc.xyz[0] 121 | acc_y = acc.xyz[1] 122 | acc_z = acc.xyz[2] + 9.8 # 9.8 offsets gravity # +: down, -: up 123 | hor_acc = math.sqrt(pow(acc_x, 2) + pow(acc_y, 2)) 124 | 125 | local_errs = list() 126 | 127 | if config.flight_mode == "POSCTL": 128 | if hor_acc > 5.0: 129 | local_errs.append(f"{ts} MPC_ACC_HOR_MAX violated: {hor_acc} > 3.0") 130 | 131 | if acc_z < -4.0: 132 | local_errs.append(f"{ts} MPC_ACC_UP_MAX violated: {acc_z} < -4.0") 133 | 134 | if acc_z > 3.0: 135 | local_errs.append(f"{ts} MPC_ACC_DOWN_MAX violated: {acc_z} > 3.0") 136 | 137 | if len(local_errs) > 0: 138 | # check if ground 139 | ts_diff = [abs(ts - f_ts) for f_ts in filter_ts_list] 140 | if len(ts_diff) == 0: # if drone was always above the ground 141 | errs.extend(local_errs) 142 | elif min(ts_diff) > 0.25 * 1000 * 1000 * 1000: # diff > 0.25 sec 143 | errs.extend(local_errs) 144 | else: 145 | pass 146 | # print("[checker] TS filtered:", local_errs) 147 | 148 | # 1.2. Velocity 149 | for (ts, pos) in vehicle_local_position_list: 150 | vx = pos.vx 151 | vy = pos.vy 152 | vz = pos.vz 153 | 154 | local_errs = list() 155 | hor_vel = math.sqrt(pow(vx, 2) + pow(vy, 2)) 156 | 157 | if config.flight_mode == "MANUAL": 158 | if hor_vel > 10.0: 159 | local_errs.append(f"{ts} MPC_VEL_MANUAL violated: {hor_vel} > 10.0") 160 | 161 | elif config.flight_mode == "POSCTL": 162 | if vz < 0 and vz < -1.0: # going down 163 | local_errs.append(f"{ts} MPC_Z_VEL_MAX_DN violated: {vz} < -1.0") 164 | elif vz > 0 and vz > 3.0: # going up 165 | local_errs.append(f"{ts} MPC_Z_VEL_MAX_UP violated: {vz} > 3.0") 166 | 167 | elif config.flight_mode == "ALTCTL": 168 | if vz < 0 and vz < -1.0: # going down 169 | local_errs.append(f"{ts} MPC_Z_VEL_MAX_DN violated: {vz} < -1.0") 170 | elif vz > 0 and vz > 3.0: # going up 171 | local_errs.append(f"{ts} MPC_Z_VEL_MAX_UP violated: {vz} > 3.0") 172 | 173 | if len(local_errs) > 0: 174 | # check if ground 175 | ts_diff = [abs(ts - f_ts) for f_ts in filter_ts_list] 176 | if len(ts_diff) == 0: # if drone was always above the ground 177 | errs.extend(local_errs) 178 | elif min(ts_diff) > 0.25 * 1000 * 1000 * 1000: # diff > 0.25 sec 179 | errs.extend(local_errs) 180 | else: 181 | pass 182 | # print("[checker] TS filtered:", local_errs) 183 | 184 | # 2. check for sensor inconsistency 185 | # 2.1. IMU discrepancy 186 | acc_inconsistency_list = list() 187 | gyro_inconsistency_list = list() 188 | for (ts, imu_state) in sensor_imu_list: 189 | acc_inconsistency_list.append(imu_state.accel_inconsistency_m_s_s[0]) 190 | gyro_inconsistency_list.append(imu_state.gyro_inconsistency_rad_s[0]) 191 | 192 | for feedback in feedback_list: 193 | if feedback.name == "imu_accel_inconsistency": 194 | feedback.update_value(max(acc_inconsistency_list)) 195 | elif feedback.name == "imu_gyro_inconsistency": 196 | feedback.update_value(max(gyro_inconsistency_list)) 197 | 198 | # 2.2. GPS discrepancy 199 | # Measure discrepancy between vehicle_gps_position (copy of raw sensor 200 | # readings) VS vehicle_global_position (EKF2 estimation) 201 | 202 | # raw gps messages are far less frequently published than the 203 | # gps estimates, thus need timestamp matching 204 | gps_matched = list() 205 | lat_diff = list() 206 | lon_diff = list() 207 | skip = 0 208 | for (ts_gps_raw, gps_raw) in vehicle_gps_list: 209 | ts_diff = 9999999999999999999 210 | last_updated = 0 211 | updated = 0 212 | 213 | for i in range(skip, len(vehicle_global_position_list)): 214 | ts_gps_estim = vehicle_global_position_list[i][0] 215 | gps_estim = vehicle_global_position_list[i][1] 216 | 217 | ts_diff_last = abs(ts_gps_raw - ts_gps_estim) 218 | if ts_diff_last <= ts_diff: 219 | ts_diff = ts_diff_last 220 | last_updated = updated 221 | updated = 1 222 | else: 223 | last_updated = updated 224 | updated = 0 225 | 226 | if last_updated == 1 and updated == 0: 227 | skip = i 228 | break 229 | 230 | gps_matched.append( 231 | ( 232 | ts_gps_raw, 233 | ts_gps_estim, 234 | gps_raw.lat, 235 | int(gps_estim.lat * 10000000), 236 | gps_raw.lon, 237 | int(gps_estim.lon * 10000000) 238 | ) 239 | ) 240 | lat_diff.append(gps_raw.lat - int(gps_estim.lat * 10000000)) 241 | lon_diff.append(gps_raw.lon - int(gps_estim.lon * 10000000)) 242 | 243 | updated = 0 244 | for feedback in feedback_list: 245 | if feedback.name == "gps_lat_inconsistency": 246 | feedback.update_value(max(lat_diff)) 247 | updated += 1 248 | elif feedback.name == "gps_lon_inconsistency": 249 | feedback.update_value(max(lon_diff)) 250 | updated += 1 251 | 252 | if updated == 2: 253 | break 254 | 255 | # 3. During parameter testing (in hold mode), drone should not move 256 | # (stay within a reasonably small area) 257 | if config.exp_pgfuzz: 258 | mean_x = statistics.mean(local_x_list) 259 | mean_y = statistics.mean(local_y_list) 260 | mean_z = statistics.mean(local_z_list) 261 | 262 | # std_x = statistics.pstdev(local_x_list) 263 | # std_y = statistics.pstdev(local_y_list) 264 | # std_z = statistics.pstdev(local_z_list) 265 | 266 | # print("mean", mean_x, mean_y, mean_z) 267 | # print("stdev", std_x, std_y, std_z) 268 | 269 | diff_x = [abs(x - mean_x) for x in local_x_list] 270 | diff_y = [abs(y - mean_y) for y in local_y_list] 271 | diff_z = [abs(z - mean_z) for z in local_z_list] 272 | 273 | # deviation over thr (m) should be considered an error 274 | # empirically set thr to 0.3 m (1 foot) 275 | thr = 0.3 276 | if max(diff_x) > thr or max(diff_y) > thr or max(diff_z) > thr: 277 | diff_str = f"x {max(diff_x):.2f} y {max(diff_y):.2f} z {max(diff_z):.2f}" 278 | errs.append(f"{ts} Position changed in hold mode: {diff_str}") 279 | 280 | return errs 281 | 282 | -------------------------------------------------------------------------------- /src/oracles/rosidl.py: -------------------------------------------------------------------------------- 1 | def check(config, msg_list, state_dict, feedback_list): 2 | errs = list() 3 | 4 | msg = msg_list[0] 5 | msg_name = type(msg).__name__ 6 | topic_name = f"/idltest_{msg_name}_out" 7 | 8 | if topic_name not in state_dict: 9 | errs.append(f"Topic {topic_name} is lost") 10 | 11 | else: 12 | # replayed by rclcpp target 13 | msg_out_list = state_dict[topic_name] 14 | 15 | if len(msg_out_list) != 1: 16 | print("[-] multiple messages replayed by idl target") 17 | exit(-1) 18 | 19 | (ts, msg_out) = msg_out_list[0] 20 | 21 | # print(msg) 22 | # print(msg_out) 23 | 24 | if "Array" not in msg_name: 25 | # == opertor exists for built-in types 26 | if msg != msg_out: 27 | errs.append("Sent and replayed messages do not match") 28 | else: 29 | # not for the Array types 30 | if len(msg.data) != len(msg_out.data): 31 | errs.append("Sent and replayed array lengths do not match") 32 | 33 | else: 34 | for i in range(len(msg.data)): 35 | if msg.data[i] != msg_out.data[i]: 36 | errs.append("Sent and replayed messages do not match") 37 | break 38 | 39 | return errs 40 | 41 | -------------------------------------------------------------------------------- /src/oracles/turtlebot.py: -------------------------------------------------------------------------------- 1 | import math 2 | import statistics 3 | import numpy as np 4 | 5 | 6 | def check(config, msg_list, state_dict, feedback_list): 7 | errs = list() 8 | 9 | try: 10 | imu_list = state_dict["/imu"] 11 | except KeyError: 12 | print("[checker] no imu data available") 13 | imu_list = list() 14 | 15 | try: 16 | odom_list = state_dict["/odom"] 17 | except KeyError: 18 | print("[checker] no odom data available") 19 | odom_list = list() 20 | 21 | try: 22 | scan_list = state_dict["/scan"] 23 | except KeyError: 24 | print("[checker] no lidar data available") 25 | scan_list = list() 26 | 27 | imu_angles = list() # for deviation 28 | 29 | ts0 = imu_list[0][0] 30 | for (ts, imu) in imu_list: 31 | # Check NaN 32 | if math.isnan(imu.linear_acceleration.x): 33 | errs.append("imu.linear_acceleration.x is NaN") 34 | 35 | if math.isnan(imu.linear_acceleration.y): 36 | errs.append("imu.linear_acceleration.y is NaN") 37 | 38 | if math.isnan(imu.linear_acceleration.z): 39 | errs.append("imu.linear_acceleration.z is NaN") 40 | 41 | if math.isnan(imu.angular_velocity.x): 42 | errs.append("imu.angular_velocity.x is NaN") 43 | 44 | if math.isnan(imu.angular_velocity.y): 45 | errs.append("imu.angular_velocity.y is NaN") 46 | 47 | if math.isnan(imu.angular_velocity.z): 48 | errs.append("imu.angular_velocity.z is NaN") 49 | 50 | # Check INF 51 | if math.isinf(imu.linear_acceleration.x): 52 | errs.append("imu.linear_acceleration.x is INF") 53 | 54 | if math.isinf(imu.linear_acceleration.y): 55 | errs.append("imu.linear_acceleration.y is INF") 56 | 57 | if math.isinf(imu.linear_acceleration.z): 58 | errs.append("imu.linear_acceleration.z is INF") 59 | 60 | if math.isinf(imu.angular_velocity.x): 61 | errs.append("imu.angular_velocity.x is INF") 62 | 63 | if math.isinf(imu.angular_velocity.y): 64 | errs.append("imu.angular_velocity.y is INF") 65 | 66 | if math.isinf(imu.angular_velocity.z): 67 | errs.append("imu.angular_velocity.z is INF") 68 | 69 | # Check max 70 | if imu.angular_velocity.z > 2.84: 71 | errs.append(f"imu.angular_velocity.z ({imu.angular_velocity.z}) exceeded max (2.84)") 72 | 73 | # robot_pose_2 = np.arctan2(siny_cosp, cosy_cosp) 74 | imu_angle_ = np.arctan2( 75 | imu.orientation.x * imu.orientation.y + imu.orientation.w * imu.orientation.z, 76 | 0.5 - imu.orientation.y * imu.orientation.y - imu.orientation.z * imu.orientation.z); 77 | imu_angles.append((ts, imu_angle_)) 78 | 79 | robot_poses_odom = list() 80 | 81 | ts0 = odom_list[0][0] 82 | for (ts, odom) in odom_list: 83 | # Check NaN 84 | if math.isnan(odom.pose.pose.position.x): 85 | errs.append("odom.pose.pose.position.x is NaN") 86 | 87 | if math.isnan(odom.pose.pose.position.y): 88 | errs.append("odom.pose.pose.position.y is NaN") 89 | 90 | if math.isnan(odom.pose.pose.position.z): 91 | errs.append("odom.pose.pose.position.z is NaN") 92 | 93 | if math.isnan(odom.pose.pose.orientation.x): 94 | errs.append("odom.pose.pose.orientation.x is NaN") 95 | 96 | if math.isnan(odom.pose.pose.orientation.y): 97 | errs.append("odom.pose.pose.orientation.y is NaN") 98 | 99 | if math.isnan(odom.pose.pose.orientation.z): 100 | errs.append("odom.pose.pose.orientation.z is NaN") 101 | 102 | if math.isnan(odom.pose.pose.orientation.w): 103 | errs.append("odom.pose.pose.orientation.w is NaN") 104 | 105 | if math.isnan(odom.twist.twist.linear.x): 106 | errs.append("odom.twist.twist.linear.x is NaN") 107 | 108 | if math.isnan(odom.twist.twist.linear.y): 109 | errs.append("odom.twist.twist.linear.y is NaN") 110 | 111 | if math.isnan(odom.twist.twist.linear.z): 112 | errs.append("odom.twist.twist.linear.z is NaN") 113 | 114 | if math.isnan(odom.twist.twist.angular.x): 115 | errs.append("odom.twist.twist.angular.x is NaN") 116 | 117 | if math.isnan(odom.twist.twist.angular.y): 118 | errs.append("odom.twist.twist.angular.y is NaN") 119 | 120 | if math.isnan(odom.twist.twist.angular.z): 121 | errs.append("odom.twist.twist.angular.z is NaN") 122 | 123 | # Check INF 124 | if math.isinf(odom.pose.pose.position.x): 125 | errs.append("odom.pose.pose.position.x is INF") 126 | 127 | if math.isinf(odom.pose.pose.position.y): 128 | errs.append("odom.pose.pose.position.y is INF") 129 | 130 | if math.isinf(odom.pose.pose.position.z): 131 | errs.append("odom.pose.pose.position.z is INF") 132 | 133 | if math.isinf(odom.pose.pose.orientation.x): 134 | errs.append("odom.pose.pose.orientation.x is INF") 135 | 136 | if math.isinf(odom.pose.pose.orientation.y): 137 | errs.append("odom.pose.pose.orientation.y is INF") 138 | 139 | if math.isinf(odom.pose.pose.orientation.z): 140 | errs.append("odom.pose.pose.orientation.z is INF") 141 | 142 | if math.isinf(odom.pose.pose.orientation.w): 143 | errs.append("odom.pose.pose.orientation.w is INF") 144 | 145 | if math.isinf(odom.twist.twist.linear.x): 146 | errs.append("odom.twist.twist.linear.x is INF") 147 | 148 | if math.isinf(odom.twist.twist.linear.y): 149 | errs.append("odom.twist.twist.linear.y is INF") 150 | 151 | if math.isinf(odom.twist.twist.linear.z): 152 | errs.append("odom.twist.twist.linear.z is INF") 153 | 154 | if math.isinf(odom.twist.twist.angular.x): 155 | errs.append("odom.twist.twist.angular.x is INF") 156 | 157 | if math.isinf(odom.twist.twist.angular.y): 158 | errs.append("odom.twist.twist.angular.y is INF") 159 | 160 | if math.isinf(odom.twist.twist.angular.z): 161 | errs.append("odom.twist.twist.angular.z is INF") 162 | 163 | # check max 164 | lin_vel = odom.twist.twist.linear.x 165 | if lin_vel > 0.22: 166 | errs.append(f"linear velocity ({lin_vel}) exceeded max (0.22)") 167 | 168 | ang_vel = odom.twist.twist.angular.z 169 | if ang_vel > 2.84: 170 | errs.append(f"angular velocity ({ang_vel}) exceeded max (2.84)") 171 | 172 | # rev-compute theta (check Odometry::publish()) 173 | robot_pose_0 = odom.pose.pose.position.x 174 | robot_pose_1 = odom.pose.pose.position.y 175 | 176 | x = odom.pose.pose.orientation.x 177 | y = odom.pose.pose.orientation.y 178 | z = odom.pose.pose.orientation.z 179 | w = odom.pose.pose.orientation.w 180 | siny_cosp = 2 * (w * z + x * y) 181 | cosy_cosp = 1 - 2 * (y * y + z * z) 182 | robot_pose_2 = np.arctan2(siny_cosp, cosy_cosp) 183 | 184 | robot_pose_ = [robot_pose_0, robot_pose_1, robot_pose_2] 185 | robot_poses_odom.append((ts, robot_pose_2)) 186 | 187 | last_theta = 0 188 | robot_poses_imu = [] 189 | delta_theta = 0.0 190 | for (ts, theta) in imu_angles: 191 | delta_theta += theta - last_theta 192 | last_theta = theta 193 | robot_poses_imu.append((ts, delta_theta)) 194 | 195 | # imu data is published approx. 4x faster than odom data 196 | # need to match the granularity 197 | theta_matched = list() # ts, odom_theta, imu_theta 198 | skip = 0 199 | for (ts_odom, odom) in robot_poses_odom: 200 | ts_diff = 9999999999999999999 201 | last_updated = 0 202 | updated = 0 203 | 204 | for i in range(skip, len(robot_poses_imu)): 205 | ts_imu = robot_poses_imu[i][0] 206 | imu = robot_poses_imu[i][1] 207 | 208 | ts_diff_last = abs(ts_odom - ts_imu) 209 | if ts_diff_last <= ts_diff: 210 | ts_diff = ts_diff_last 211 | last_updated = updated 212 | updated = 1 213 | else: 214 | last_updated = updated 215 | updated = 0 216 | 217 | if last_updated == 1 and updated == 0: 218 | skip = i 219 | break 220 | 221 | theta_matched.append((ts_odom, ts_imu, odom, imu)) 222 | 223 | theta_diff = list() 224 | for tup in theta_matched: 225 | # print(tup[0], tup[1], tup[2], tup[3], abs(tup[2] - tup[3])) 226 | theta_diff.append(abs(tup[2] - tup[3])) 227 | 228 | # todo: normalize diff, get anomaly and translate to feedback!! 229 | min_diff = min(theta_diff) 230 | max_diff = max(theta_diff) 231 | mean = statistics.mean(theta_diff) 232 | median = statistics.median(theta_diff) 233 | 234 | for feedback in feedback_list: 235 | if feedback.name == "theta_diff": 236 | feedback.update_value(max_diff) 237 | break 238 | 239 | if max_diff > 5.0: 240 | errs.append(f"theta estimation error is too huge: {max_diff}") 241 | 242 | for (ts, scan) in scan_list: 243 | found_nan = False 244 | range_error = False 245 | 246 | for scan_range in scan.ranges: 247 | if math.isnan(scan_range): 248 | found_nan = True 249 | break 250 | 251 | elif scan_range < 0 or scan_range > 65.535: 252 | range_error = True 253 | break 254 | 255 | if found_nan: 256 | errs.append("scan.range contains NaN") 257 | 258 | if range_error: 259 | errs.append(f"scan.range {scan_range} is out of range") 260 | 261 | return errs 262 | 263 | -------------------------------------------------------------------------------- /src/oracles/turtlesim.py: -------------------------------------------------------------------------------- 1 | import math 2 | 3 | 4 | def check(config, msg_list, pose_list, feedback_list): 5 | errs = list() 6 | 7 | for (ts, pose) in pose_list: 8 | if pose.x < 0: 9 | errs.append("pose.x is below minimum") 10 | 11 | if pose.x >= 11.09: 12 | errs.append("pose.x is above maximum") 13 | 14 | if pose.y < 0: 15 | errs.append("pose.y is below minimum") 16 | 17 | if pose.y >= 11.09: 18 | errs.append("pose.y is above maximum") 19 | 20 | if pose.theta >= 3.141593: 21 | errs.append("pose.theta is above maximum") 22 | 23 | if pose.theta <= -3.141593: 24 | errs.append("pose.theta is below minimum") 25 | 26 | if math.isnan(pose.x): 27 | errs.append("pose.x is NaN") 28 | 29 | if math.isnan(pose.y): 30 | errs.append("pose.y is NaN") 31 | 32 | if math.isnan(pose.theta): 33 | errs.append("pose.theta is NaN") 34 | 35 | if math.isnan(pose.linear_velocity): 36 | errs.append("pose.linear_velocity is NaN") 37 | 38 | if math.isnan(pose.angular_velocity): 39 | errs.append("pose.anglar_velocity is NaN") 40 | 41 | if math.isinf(pose.x): 42 | errs.append("pose.x is INF") 43 | 44 | if math.isinf(pose.y): 45 | errs.append("pose.y is INF") 46 | 47 | if math.isinf(pose.theta): 48 | errs.append("pose.theta is INF") 49 | 50 | if math.isinf(pose.linear_velocity): 51 | errs.append("pose.linear_velocity is INF") 52 | 53 | if math.isinf(pose.angular_velocity): 54 | errs.append("pose.angular_velocity is INF") 55 | 56 | return errs 57 | 58 | -------------------------------------------------------------------------------- /src/policies/common/lifecycle_node.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | ~/change_state 8 | ~/get_available_states 9 | ~/get_available_transitions 10 | ~/get_state 11 | ~/get_transition_graph 12 | 13 | 14 | ~/transition_event 15 | 16 | 17 | -------------------------------------------------------------------------------- /src/policies/common/node.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 7 | 9 | 10 | -------------------------------------------------------------------------------- /src/policies/common/node/logging.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | rosout 5 | 6 | 7 | -------------------------------------------------------------------------------- /src/policies/common/node/parameters.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | /parameter_events 5 | 6 | 7 | 8 | ~/describe_parameters 9 | ~/get_parameter_types 10 | ~/get_parameters 11 | ~/list_parameters 12 | ~/set_parameters 13 | ~/set_parameters_atomically 14 | 15 | 16 | -------------------------------------------------------------------------------- /src/policies/common/node/time.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | /clock 5 | 6 | 7 | -------------------------------------------------------------------------------- /src/policies/fuzzer.policy.xml: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 7 | 8 | 9 | 10 | 11 | sros2_input 12 | sros2_output 13 | _listen_flag 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /src/policies/sros2_node.policy.xml: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | 7 | 8 | 10 | 11 | sros2_input 12 | sros2_output 13 | _listen_flag 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /src/px4_prep/blacklist.py: -------------------------------------------------------------------------------- 1 | # irrelevant or tested parameters 2 | # manage statically for now 3 | blacklist = [ 4 | "CAL_ACC0_XOFF", # calibrates the sensor offset 5 | "CAL_ACC0_YOFF", 6 | "CAL_ACC0_ZOFF", 7 | "CAL_ACC1_XOFF", 8 | "CAL_ACC1_YOFF", 9 | "CAL_ACC1_ZOFF", 10 | "CAL_ACC2_XOFF", 11 | "CAL_ACC2_YOFF", 12 | "CAL_ACC2_ZOFF", 13 | "CAL_GYRO0_XOFF", 14 | "CAL_GYRO0_YOFF", 15 | "CAL_GYRO0_ZOFF", 16 | "CAL_GYRO1_XOFF", 17 | "CAL_GYRO1_YOFF", 18 | "CAL_GYRO1_ZOFF", 19 | "CAL_GYRO2_XOFF", 20 | "CAL_GYRO2_YOFF", 21 | "CAL_GYRO2_ZOFF", 22 | "CAL_MAG0_XOFF", 23 | "CAL_MAG0_YOFF", 24 | "CAL_MAG0_ZOFF", 25 | "CAL_MAG1_XOFF", 26 | "CAL_MAG1_YOFF", 27 | "CAL_MAG1_ZOFF", 28 | "EKF2_GPS_POS_X", # doesn't make sense to restrict these (hw-dependent) 29 | "EKF2_GPS_POS_Y", 30 | "EKF2_GPS_POS_Z", 31 | "EKF2_IMU_POS_X", 32 | "EKF2_IMU_POS_Y", 33 | "EKF2_IMU_POS_Z", 34 | ] 35 | 36 | # already confirmed as buggy 37 | tested = [ 38 | "MC_PITCHRATE_D", 39 | "MC_PITCHRATE_FF", 40 | "MC_PITCHRATE_I", 41 | 42 | ] 43 | -------------------------------------------------------------------------------- /src/px4_prep/params.pkl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sslab-gatech/RoboFuzz/d4199bb834242441056fc10f0f51055cf7910416/src/px4_prep/params.pkl -------------------------------------------------------------------------------- /src/px4_prep/preprocess_params.py: -------------------------------------------------------------------------------- 1 | import pickle 2 | from bs4 import BeautifulSoup 3 | 4 | 5 | f1 = open("../PARAMS_IN_USE.list", "r") 6 | 7 | param_list = list() 8 | 9 | lines = f1.readlines() 10 | for line in lines: 11 | if line.startswith("x"): 12 | name = line.replace("+", " ").replace("*", " ")[4:].split(" ")[0] 13 | param_list.append(name) 14 | 15 | f1.close() 16 | 17 | 18 | f = open("./parameter_reference.md", "r") 19 | doc = f.read() 20 | f.close() 21 | 22 | soup = BeautifulSoup(doc, "html.parser") 23 | 24 | tables = soup.find_all("table") 25 | print(len(tables)) 26 | 27 | def find_param_in_table(param_name, table): 28 | strong = table.find(id=param_name) 29 | return strong 30 | 31 | 32 | param_dict = dict() 33 | 34 | for param in param_list: 35 | for table in tables: 36 | tag = find_param_in_table(param, table) 37 | if tag is not None: 38 | param_name = tag["id"] 39 | 40 | param_type = tag.parent.text.split("(")[1].split(")")[0] 41 | 42 | tr = tag.parent.parent 43 | tds = tr.find_all("td") 44 | range_str = tds[2].text 45 | default_str = tds[3].text 46 | unit_str = tds[4].text 47 | 48 | range_str = range_str.split("(")[0] 49 | if ">" in range_str: 50 | range_min = range_str.split(" > ")[0].strip() 51 | range_max = range_str.split(" > ")[1].strip() 52 | if range_min == "?": 53 | range_min = None 54 | else: 55 | if param_type == "INT32": 56 | range_min = int(range_min) 57 | elif param_type == "FLOAT": 58 | range_min = float(range_min) 59 | 60 | if range_max == "?": 61 | range_max = None 62 | else: 63 | if param_type == "INT32": 64 | range_max = int(range_max) 65 | elif param_type == "FLOAT": 66 | range_max = float(range_max) 67 | 68 | else: 69 | range_min = None 70 | range_max = None 71 | 72 | if range_min == "?": 73 | range_min = None 74 | if range_max == "?": 75 | range_max = None 76 | 77 | if "able" in default_str: 78 | # 0 or 1 79 | default_val = int(default_str.split("(")[-1].split(")")[0]) 80 | else: 81 | default_val = default_str 82 | 83 | if param_type == "INT32": 84 | default_val = int(default_val) 85 | elif param_type == "FLOAT": 86 | default_val = float(default_val) 87 | 88 | param_dict[param_name] = { 89 | "type": param_type, 90 | "min": range_min, 91 | "max": range_max, 92 | "default": default_val, 93 | "unit": unit_str 94 | } 95 | 96 | # print(f"{param_name} / {param_type} / {range_min} {range_max} / {default_val}") 97 | 98 | break 99 | 100 | pickle.dump(param_dict, open("params.pkl", "wb")) 101 | 102 | 103 | 104 | 105 | -------------------------------------------------------------------------------- /src/px4_ros_bridge.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | 3 | import time 4 | import sys 5 | import random 6 | 7 | import px4_utils 8 | 9 | import rclpy 10 | from px4_msgs.msg import Timesync, VehicleCommand, TrajectorySetpoint, OffboardControlMode 11 | 12 | 13 | class Node: 14 | def __init__(self, name): 15 | self.node_handle = rclpy.create_node(name) 16 | self.sub_timesync = self.node_handle.create_subscription( 17 | Timesync, "Timesync_PubSubTopic", self.timesync_callback, 10) 18 | self.bit = 0 19 | 20 | def timesync_callback(self, msg): 21 | self.ts = msg.timestamp 22 | print("ts:", self.ts) 23 | 24 | def init_publisher(self): 25 | self.pub_offboard_control_mode = self.node_handle.create_publisher( 26 | OffboardControlMode, "/OffboardControlMode_PubSubTopic", 10) 27 | self.pub_vehiclecommand = self.node_handle.create_publisher( 28 | VehicleCommand, "/VehicleCommand_PubSubTopic", 10) 29 | self.pub_trajectory = self.node_handle.create_publisher( 30 | TrajectorySetpoint, "/TrajectorySetpoint_PubSubTopic", 10) 31 | 32 | def publish_offboard_mode(self): 33 | msg = VehicleCommand() 34 | msg.command = VehicleCommand.VEHICLE_CMD_DO_SET_MODE 35 | msg.timestamp = self.ts 36 | msg.param1 = 1.0 37 | msg.param2 = 6.0 38 | msg.target_system = 1 39 | msg.target_component = 1 40 | msg.source_system = 1 41 | msg.source_component = 1 42 | msg.from_external = True 43 | print(msg) 44 | self.pub_vehiclecommand.publish(msg) 45 | 46 | def publish_offboard_control_mode(self): 47 | msg = OffboardControlMode() 48 | msg.timestamp = self.ts 49 | msg.position = True 50 | msg.velocity = True 51 | msg.acceleration = True 52 | msg.attitude = True 53 | msg.body_rate = True 54 | print(msg) 55 | self.pub_offboard_control_mode.publish(msg) 56 | 57 | 58 | def publish_arm_command(self): 59 | msg = VehicleCommand() 60 | msg.command = VehicleCommand.VEHICLE_CMD_COMPONENT_ARM_DISARM 61 | msg.timestamp = self.ts 62 | msg.param1 = 1.0 63 | msg.target_system = 1 64 | msg.target_component = 1 65 | msg.source_system = 1 66 | msg.source_component = 1 67 | msg.from_external = True 68 | print(msg) 69 | self.pub_vehiclecommand.publish(msg) 70 | 71 | def publish_trajectory(self, i, home=False, msg=None): 72 | 73 | if msg is None: 74 | # manual testing 75 | msg = TrajectorySetpoint() 76 | msg.timestamp = self.ts 77 | 78 | # msg.x = float(random.randint(-1000, 1000)) / random.randint(1, 5) 79 | # msg.y = float(random.randint(-1000, 1000)) / random.randint(1, 5) 80 | # msg.z = float(random.randint(-100, -1)) / random.randint(1, 5) 81 | if home: 82 | msg.x = 0.0 83 | msg.y = 0.0 84 | msg.z = -5.0 85 | msg.yaw = 0.0 86 | 87 | else: 88 | msg.x = 10.0 89 | msg.y = 10.0 90 | msg.z = -5.0 91 | if i == 50: 92 | print("**************************************************************************") 93 | print("**************************************************************************") 94 | print("**************************************************************************") 95 | print("**************************************************************************") 96 | print("**************************************************************************") 97 | print("**************************************************************************") 98 | msg.vx = 0.0 99 | msg.acceleration[0] = 0.0 100 | # msg.vx = 0.0 101 | # msg.acceleration[0] = 90000000.0 102 | else: 103 | msg.vx = 0.0 104 | msg.vy = 0.0 105 | # msg.acceleration[0] = 3000.0 106 | 107 | # if self.bit: 108 | # msg.yaw = -3.14 109 | # else: 110 | # msg.yaw = 0.0 111 | # self.bit ^= 1 112 | msg.yaw = 3.14 113 | msg.yawspeed = 0.0 114 | 115 | print(msg) 116 | self.pub_trajectory.publish(msg) 117 | 118 | 119 | def main(msg_list): 120 | 121 | rclpy.init() 122 | node = Node("sender") 123 | node.init_publisher() 124 | 125 | # https://docs.px4.io/master/en/flight_modes/offboard.html#offboard-mode 126 | # The vehicle must be already be receiving a stream of target setpoints 127 | # (>2Hz) before this mode can be engaged. 128 | for i in range(10): 129 | rclpy.spin_once(node.node_handle) 130 | node.publish_offboard_control_mode() 131 | node.publish_trajectory(i) 132 | time.sleep(0.1) 133 | 134 | rclpy.spin_once(node.node_handle) 135 | node.publish_offboard_mode() 136 | time.sleep(0.1) 137 | 138 | # The vehicle must be armed before this mode can be engaged. 139 | rclpy.spin_once(node.node_handle) 140 | node.publish_arm_command() 141 | time.sleep(0.1) 142 | 143 | # The vehicle will exit the mode if target setpoints are not received at a 144 | # rate of > 2Hz. 145 | 146 | if msg_list: 147 | # given a mission 148 | for msg in msg_list: 149 | rclpy.spin_once(node.node_handle) 150 | node.publish_offboard_control_mode() 151 | node.publish_trajectory(i=0, home=False, msg=msg) 152 | time.sleep(0.1) 153 | 154 | else: 155 | # manual testing 156 | for i in range(100): 157 | rclpy.spin_once(node.node_handle) 158 | node.publish_offboard_control_mode() 159 | node.publish_trajectory(i) 160 | time.sleep(0.1) 161 | 162 | for i in range(100): 163 | rclpy.spin_once(node.node_handle) 164 | node.publish_offboard_control_mode() 165 | node.publish_trajectory(i, home=True) 166 | time.sleep(0.1) 167 | 168 | node.node_handle.destroy_node() 169 | rclpy.shutdown() 170 | 171 | 172 | if __name__ == "__main__": 173 | trajectory_msg_list = None 174 | 175 | if len(sys.argv) > 1: 176 | trajectory_msg_list = px4_utils.read_trajectory_seed(sys.argv[1]) 177 | if trajectory_msg_list is None: 178 | print("[-] wrong file") 179 | sys.exit(1) 180 | 181 | main(trajectory_msg_list) 182 | -------------------------------------------------------------------------------- /src/qos_override.yaml: -------------------------------------------------------------------------------- 1 | /listen_flag: 2 | reliability: reliable 3 | durability: transient_local 4 | history: keep_last 5 | depth: 30 6 | -------------------------------------------------------------------------------- /src/rcl_harness.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | import time 4 | import subprocess as sp 5 | import signal 6 | import argparse 7 | 8 | import rclpy 9 | from rclpy.node import Node 10 | from rclpy.action import ActionServer 11 | 12 | from std_msgs.msg import String 13 | from tracer import APITracer 14 | 15 | 16 | def parse_cov(covfile): 17 | with open(covfile, "r") as f: 18 | line = f.read() 19 | 20 | branches = set(line.split(" ")) 21 | 22 | return branches 23 | 24 | 25 | def test_cov_diff(): 26 | cwd = os.getcwd() 27 | rcl_target_dir = os.path.join(cwd, "../targets/rcl_lang_ws/build") 28 | 29 | targets = [ 30 | "python3 " 31 | + os.path.join( 32 | rcl_target_dir, 33 | "rclpy_publisher/rclpy_publisher/rclpy_publisher.py", 34 | ), 35 | os.path.join(rcl_target_dir, "rclcpp_publisher/rclcpp_publisher"), 36 | os.path.join( 37 | rcl_target_dir, 38 | "rclrs_publisher/ament_cargo/rclrs_publisher/target/release", 39 | ), 40 | ] 41 | 42 | for i, target in enumerate(targets): 43 | cmd = "COVFILE_PATH=./cov-{} {}".format(i, target) 44 | print(target) 45 | os.system(cmd) 46 | 47 | branches = [] 48 | for i in range(len(targets)): 49 | branches.append(parse_cov(f"cov-{i}")) 50 | 51 | a = branches[0] 52 | b = branches[1] 53 | 54 | c = a.difference(b) 55 | d = b.difference(a) 56 | u = a.union(b) 57 | i = a.intersection(b) 58 | 59 | print("union", len(u)) 60 | print("intersection", len(i)) 61 | print("c", len(c)) 62 | print("d", len(d)) 63 | 64 | 65 | class RCLSubscriber(Node): 66 | def __init__(self, features, targets, job): 67 | super().__init__("rcl_harness") 68 | self.subscription = self.create_subscription( 69 | String, "rcl_topic", self.rcl_harness_callback, 10 70 | ) 71 | self.features = features 72 | self.job = job 73 | 74 | cwd = os.getcwd() 75 | rcl_target_dir = os.path.join(cwd, "../targets/rcl_lang_ws/build") 76 | 77 | self.api_list = [] 78 | for feature in features: 79 | with open(f"librcl_apis/{feature}.txt", "r") as f: 80 | self.api_list.extend(f.readlines()) 81 | 82 | for i in range(len(self.api_list)): 83 | self.api_list[i] = self.api_list[i].strip() + "@librcl.so" 84 | 85 | # set blacklist, whitelist 86 | 87 | self.targets = [] 88 | if "py" in targets: 89 | self.targets.append( 90 | "python3 " 91 | + os.path.join( 92 | rcl_target_dir, 93 | f"rclpy_{job}/rclpy_{job}/rclpy_{job}.py", 94 | ) 95 | ) 96 | if "cpp" in targets: 97 | self.targets.append( 98 | os.path.join(rcl_target_dir, f"rclcpp_{job}/rclcpp_{job}") 99 | ) 100 | if "rs" in targets: 101 | self.targets.append( 102 | os.path.join( 103 | rcl_target_dir, 104 | f"rclrs_{job}/ament_cargo/rclrs_{job}/target/release/rclrs_{job}", 105 | ) 106 | ) 107 | 108 | def rcl_harness_callback(self, msg): 109 | print(f"[rcl harness] Testing {self.features}") 110 | 111 | self.api_list.insert(0, "") 112 | lib_filter = " -x ".join(self.api_list) 113 | self.api_list.remove("") 114 | 115 | pgrp = None 116 | if "subscriber" in self.features: 117 | # need to re-echo data to topic as subscribers are waiting 118 | cmd = "ros2 topic pub /topic std_msgs/String '{data: %s}'" % ( 119 | msg.data 120 | ) 121 | pgrp = sp.Popen( 122 | cmd, 123 | shell=True, 124 | preexec_fn=os.setpgrp, 125 | stdout=sp.DEVNULL, 126 | stderr=sp.DEVNULL, 127 | ) 128 | 129 | # exec each rcl lang binaries 130 | for i, target in enumerate(self.targets): 131 | print(f"[rcl harness] Executing {target}") 132 | 133 | outfile = f"out-{i}" 134 | tracefile = f"trace-{i}" 135 | try: 136 | os.remove(outfile) 137 | except: 138 | pass 139 | try: 140 | os.remove(tracefile) 141 | except: 142 | pass 143 | 144 | cmd = f"ltrace -o {tracefile} -ttt {lib_filter} {target} {msg.data} > {outfile}" 145 | os.system(cmd) 146 | 147 | if pgrp is not None: 148 | try: 149 | os.killpg(pgrp.pid, signal.SIGKILL) 150 | except: 151 | print("[-] could not kill ros2 topic pub") 152 | 153 | try: 154 | os.remove(".waitlock") 155 | except: 156 | pass 157 | 158 | 159 | def main(features, targets, job): 160 | rclpy.init() 161 | 162 | node = RCLSubscriber(features, targets, job) 163 | 164 | rclpy.spin_once(node, timeout_sec=20) 165 | 166 | node.destroy_node() 167 | rclpy.shutdown() 168 | 169 | 170 | if __name__ == "__main__": 171 | argparser = argparse.ArgumentParser() 172 | 173 | argparser.add_argument( 174 | "-f", 175 | type=str, 176 | nargs="+", 177 | ) 178 | argparser.add_argument( 179 | "-t", 180 | type=str, 181 | nargs="+", 182 | ) 183 | argparser.add_argument( 184 | "-j", 185 | type=str, 186 | ) 187 | 188 | args = argparser.parse_args() 189 | 190 | main(args.f, args.t, args.j) 191 | -------------------------------------------------------------------------------- /src/reproduce_mav.py: -------------------------------------------------------------------------------- 1 | import os 2 | import re 3 | import pickle 4 | import time 5 | import json 6 | import signal 7 | import subprocess as sp 8 | import shutil 9 | 10 | import px4_utils 11 | import checker 12 | from rosbag_parser import RosbagParser 13 | 14 | 15 | class Msg: 16 | """a dummy message class for compatibility with px4_utils""" 17 | def __init__(self, x, y, z, r): 18 | self.x = x 19 | self.y = y 20 | self.z = z 21 | self.r = r 22 | 23 | class Config: 24 | """a dummy config class for compatibility with checker""" 25 | px4_sitl = True 26 | rospkg = None 27 | tb3_sitl = False 28 | tb3_hitl = False 29 | test_rosidl = False 30 | 31 | 32 | def get_subframe(filename): 33 | return filename.split("-")[-1] 34 | 35 | 36 | def start_rosbag(frame): 37 | watchlist_file = "watchlist/px4.json" 38 | bag_dir = f"states-replay-{frame}.bag" 39 | print("bag:", bag_dir) 40 | 41 | with open(watchlist_file, "r") as fp: 42 | data = fp.read() 43 | 44 | try: 45 | shutil.rmtree(bag_dir) 46 | except: 47 | print("[executor] could not remove previous bag dir") 48 | 49 | rosbag_cmd = f"ros2 bag record -o {bag_dir} --qos-profile-overrides-path qos_override.yaml " 50 | watchlist = json.loads(data) 51 | for target in watchlist: 52 | rosbag_cmd += target + " " 53 | rosbag_cmd += "listen_flag" 54 | 55 | rosbag_pgrp = sp.Popen( 56 | rosbag_cmd, 57 | shell=True, 58 | preexec_fn=os.setpgrp, 59 | stdout=sp.PIPE, 60 | stderr=sp.PIPE, 61 | ) 62 | print("[executor] started ros2 bag recording") 63 | # print(rosbag_cmd) 64 | 65 | # need time for topics to be discovered 66 | time.sleep(1) 67 | 68 | return rosbag_pgrp 69 | 70 | 71 | def start_watching(): 72 | print("start watching", time.time()) 73 | 74 | sp.call( 75 | [ 76 | "ros2", 77 | "topic", 78 | "pub", 79 | "--once", 80 | "/listen_flag", 81 | "std_msgs/UInt64", 82 | "{data: " + str(int(str(time.time()).replace(".", ""))) + "}", 83 | ], 84 | stdout=sp.DEVNULL, 85 | stderr=sp.DEVNULL, 86 | ) 87 | 88 | def stop_watching(): 89 | print("stop watching", time.time()) 90 | 91 | sp.call( 92 | [ 93 | "ros2", 94 | "topic", 95 | "pub", 96 | "--once", 97 | "/listen_flag", 98 | "std_msgs/UInt64", 99 | "{data: " + str(int(str(time.time()).replace(".", ""))) + "}", 100 | ], 101 | stdout=sp.DEVNULL, 102 | stderr=sp.DEVNULL, 103 | ) 104 | 105 | 106 | log_dir = "logs/20220104-204610/" 107 | frame = "1641356645.0672185" 108 | 109 | log_dir = "logs/20220105-165934/" 110 | frame = "1641419985.117267" 111 | 112 | # interesting 113 | log_dir = "logs/20220105-193319/" 114 | frame = "1641444258.535236" 115 | log_dir = "logs/20220105-193319/" 116 | # frame = "1641443932.2358694" 117 | frame = "1641441647.26109" 118 | log_dir = "logs/20220128-122952" 119 | frame = "1643407191.4075294" 120 | 121 | msg_file_list = [ 122 | f 123 | for f in os.listdir(os.path.join(log_dir, "queue")) 124 | if re.match(r'msg-' + frame + r'-*', f) 125 | ] 126 | 127 | msg_file_list.sort(key=get_subframe) 128 | msg_list = list() 129 | 130 | for msg in msg_file_list: 131 | msg_dict = pickle.load(open(os.path.join(log_dir, "queue", msg), "rb")) 132 | msg_dict = dict(msg_dict) 133 | msg = Msg(msg_dict["x"], msg_dict["y"], msg_dict["z"], msg_dict["r"]) 134 | msg_list.append(msg) 135 | 136 | px4_bridge = px4_utils.Px4BridgeNode() 137 | px4_bridge.init_mavlink() 138 | px4_bridge.prepare_flight() 139 | 140 | rosbag_pgrp = start_rosbag(frame) 141 | start_watching() 142 | 143 | for msg in msg_list: 144 | # print(msg.x, msg.y, msg.z, msg.r) 145 | px4_bridge.send_command(msg) 146 | time.sleep(0.1) 147 | 148 | stop_watching() 149 | os.killpg(rosbag_pgrp.pid, signal.SIGINT) 150 | 151 | 152 | parser = RosbagParser( 153 | f"states-replay-{frame}.bag/states-replay-{frame}.bag_0.db3" 154 | ) 155 | state_msgs_dict = parser.process_messages() 156 | 157 | config = Config() 158 | 159 | errs = checker.run_checks(config, msg_list, state_msgs_dict) 160 | print(errs) 161 | 162 | 163 | -------------------------------------------------------------------------------- /src/requirements.txt: -------------------------------------------------------------------------------- 1 | sysv-ipc==1.1.0 2 | kinpy==0.1.0 3 | -------------------------------------------------------------------------------- /src/ros2_fuzzer/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sslab-gatech/RoboFuzz/d4199bb834242441056fc10f0f51055cf7910416/src/ros2_fuzzer/__init__.py -------------------------------------------------------------------------------- /src/ros2_fuzzer/process_handling.py: -------------------------------------------------------------------------------- 1 | """ 2 | ROS Fuzzer process health handling module. 3 | 4 | :authors: Alias Robotics S.L. Borja Erice, Odei Olalde, Xabi Perez, Gorka Olalde 5 | """ 6 | import rclpy 7 | 8 | 9 | class FuzzedNodeHandler: 10 | 11 | def __init__(self, node_name): 12 | self.node_name = node_name 13 | try: 14 | rclpy.init() 15 | except: 16 | pass 17 | self.node = rclpy.create_node('health_checker') 18 | 19 | def check_if_alive(self): 20 | nodes, namespaces = self.node.get_node_names() 21 | return self.node_name in nodes 22 | 23 | -------------------------------------------------------------------------------- /src/ros2_fuzzer/ros_basic_strategies.py: -------------------------------------------------------------------------------- 1 | """ 2 | ROS Fuzzer basic datatype strategies module. 3 | 4 | :authors: Alias Robotics S.L. Borja Erice, Odei Olalde, Xabi Perez, Gorka Olalde 5 | """ 6 | import hypothesis.strategies as st 7 | from hypothesis.errors import InvalidArgument 8 | from collections import namedtuple 9 | 10 | STRING_MIN_SIZE = 0 11 | """int: Minimal string size.""" 12 | STRING_MAX_SIZE = 1000 13 | """int: Maximal string size.""" 14 | 15 | 16 | @st.defines_strategy 17 | def string(min_size=STRING_MIN_SIZE, max_size=STRING_MAX_SIZE): 18 | """ 19 | Generate value for ROS builtin message type "string". 20 | 21 | :param min_size: int 22 | Minimal size to generate 23 | :param max_size: int 24 | Maximal size to generate 25 | :return: :func:`hypothesis.strategies.binary()` 26 | Strategy with preconfigured default values. 27 | """ 28 | if not STRING_MIN_SIZE <= min_size <= max_size <= STRING_MAX_SIZE: 29 | raise InvalidArgument 30 | # average_size parameter is deprecated 31 | return st.text(min_size=min_size, max_size=max_size) 32 | 33 | 34 | @st.composite 35 | def time(draw, secs=st.integers(), nsecs=st.integers()): 36 | """ 37 | Generate value for ROS builtin message type "time". 38 | 39 | :param secs: hypothesis_ros.message_fields.uint32() 40 | Seconds 41 | :param nsecs: hypothesis_ros.message_fields.uint32() 42 | Nano seconds 43 | :return: :class:`hypothesis_ros.message_fields.Time()` 44 | Strategy with preconfigured default values. 45 | """ 46 | _Time = namedtuple('Time', 'secs nsecs') 47 | secs_value, nsecs_value = draw(secs), draw(nsecs) 48 | assert isinstance(secs_value, int), 'drew invalid sec={secs_value} from {secs} for integer field'.\ 49 | format(secs_value, secs) 50 | assert isinstance(nsecs_value, int), 'drew invalid nsec={nsecs_value} from {nsecs} for integer field'.\ 51 | format(nsecs_value, nsecs) 52 | return _Time(secs_value, nsecs_value) 53 | 54 | 55 | @st.composite 56 | def duration(draw, secs=st.integers(), nsecs=st.integers()): 57 | """ 58 | Generate value for ROS builtin message type "duration". 59 | 60 | :param secs: hypothesis_ros.message_fields.int32() 61 | Seconds 62 | :param nsecs: hypothesis_ros.message_fields.int32() 63 | Nano seconds 64 | :return: :class:`hypothesis_ros.message_fields.Duration()` 65 | Namedtuple with drawn values for secs and nsecs. 66 | """ 67 | _Duration = namedtuple('Duration', 'secs nsecs') 68 | secs_value, nsecs_value = draw(secs), draw(nsecs) 69 | assert isinstance(secs_value, int), 'drew invalid sec={secs_value} from {secs} for integer field'.\ 70 | format(secs_value, secs) 71 | assert isinstance(nsecs_value, int), 'drew invalid nsec={nsecs_value} from {nsecs} for integer field'.\ 72 | format(nsecs_value, nsecs) 73 | return _Duration(secs_value, nsecs_value) 74 | 75 | 76 | # @st.defines_strategy 77 | def array(elements=None, min_size=None, max_size=None, unique_by=None, unique=None): 78 | """ 79 | Generate variable length array with ROS builtin message types as elements. 80 | To generate a fixed length array define `min_size == max_size`. 81 | 82 | :param elements: hypothesis_ros.message_fields 83 | Strategies for pritive types from hypothesis_ros.message_fields. 84 | :param min_size: integer 85 | Minimal size of the array. 86 | :param max_size: integer 87 | Maximal size of the array. 88 | :param unique_by: callable 89 | Function returning a hashable type when given a value from elements. 90 | The resulting array (list) will satisfy `unique_by(result[i]) != unique_by(result[j])`. 91 | :param unique: boolean 92 | Function returning a hashable type. For comparison of directy object equality. 93 | :return: :func:`hypothesis.strategies.lists()` 94 | A variable or fixed length array containing values drawn from elements with 95 | length in the interval [min_size, max_size] (no bounds in that direction 96 | if these are None). 97 | """ 98 | # TODO: Assert that strategy for elements is from supported strategies. 99 | # if not min_size <= max_size: raise InvalidArgument 100 | return st.lists(elements=elements, min_size=min_size, max_size=max_size, unique_by=unique_by, unique=unique) 101 | -------------------------------------------------------------------------------- /src/ros2_fuzzer/ros_commons.py: -------------------------------------------------------------------------------- 1 | """ 2 | ROS Fuzzer base module. 3 | 4 | :authors: Alias Robotics S.L. Borja Erice, Odei Olalde, Xabi Perez, Gorka Olalde 5 | """ 6 | import importlib 7 | import re 8 | import numpy as np 9 | # import hypothesis.extra.numpy as npst 10 | # import hypothesis.strategies as st 11 | import rclpy 12 | from rclpy.node import Node 13 | # from ros2_fuzzer.ros_basic_strategies import array, string 14 | 15 | 16 | class ROS2NodeFuzzer(Node): 17 | """ 18 | Class Helper to create ROS2 Nodes and execute common actions 19 | Defined context manager to start and kill the node when finished with its execution 20 | """ 21 | def __init__(self): 22 | self.pub = None 23 | self.client = None 24 | 25 | def __enter__(self): 26 | rclpy.init() 27 | super().__init__('ROS2_Fuzzer') 28 | return self 29 | 30 | def __exit__(self, exc_type, exc_val, exc_tb): 31 | if self.pub: 32 | self.pub.destroy() 33 | if self.client: 34 | self.client.destroy() 35 | rclpy.shutdown() 36 | 37 | def publish(self, msg_type, msg, topic): 38 | if self.pub: 39 | self.pub.publish(msg) 40 | else: 41 | self.pub = self.create_publisher(msg_type, topic) 42 | self.publish(msg_type, msg, topic) 43 | 44 | def send_request(self, srv_type, srv_name, srv_request): 45 | if self.client: 46 | while not self.client.wait_for_service(timeout_sec=1.0): 47 | self.get_logger().info('service not available, waiting again...') 48 | future = self.client.call_async(srv_request) 49 | rclpy.spin_until_future_complete(self, future) 50 | else: 51 | self.client = self.create_client(srv_type, srv_name) 52 | self.send_request(srv_type, srv_name, srv_request) 53 | 54 | 55 | def ros_type_to_dict(msg_type): 56 | """ 57 | Create a dictionary which values say if the ROS2 message type is complex (not basic), which is its parent 58 | ROS message module, its type, if it is an array and if so, its size. 59 | 60 | :param msg_type: ROS2 message type. 61 | :return: A dictionary which values say if the ROS2 message type is complex (not basic), which is its parent 62 | ROS2 message module, its type, if it is an array and if so, its size. 63 | """ 64 | type_regexp = re.compile( 65 | r'^((?P[\w]+)\<|(?P(?P[\w]+)/)?(?P[\w]+)|(?P\[(?P[0-9]*)?\])?|\>)+$') 66 | type_match = type_regexp.match(msg_type) 67 | if type_match: 68 | return type_match.groupdict() 69 | else: 70 | return None 71 | 72 | 73 | # TODO possible duplicated functions ros_msg_loader and ros_srv_loader 74 | # (srv loader called once, msg_loader called more than once) 75 | def ros_msg_loader(type_dict): 76 | """ 77 | Dynamically import ROS2 message modules. 78 | 79 | :param type_dict: A dictionary which values say if the ROS message type is 80 | complex (not basic), which is its parent ROS2 message 81 | module, its type, if it is an array and if so, its size. 82 | :return: The ROS2 message class. If the provided type does not exist, 83 | raises an import error. 84 | """ 85 | try: 86 | module = importlib.import_module(type_dict['module'] + '.msg') 87 | return module.__dict__[type_dict['type']] 88 | except KeyError: 89 | raise KeyError('ROS2 message type: {} not included in message module: {}'.format(type_dict['type'], 90 | type_dict['module'])) 91 | except ImportError: 92 | raise ImportError('ROS2 message module: {} does not exist.'.format(type_dict['module'])) 93 | except TypeError: 94 | raise TypeError('ROS2 message type: {} does not exist'.format(type_dict['type'])) 95 | 96 | 97 | def ros_srv_loader(type_dict): 98 | """ 99 | Dynamically import ROS2 service modules. 100 | 101 | :param type_dict: A dictionary which values say if the ROS service type is complex (not basic), which is its parent 102 | ROS2 service module, its type, if it is an array and if so, its size. 103 | :return: The ROS2 service class. If the provided type does not exist, raises an import error. 104 | """ 105 | try: 106 | module = importlib.import_module(type_dict['module'] + '.srv') 107 | return module.__dict__[type_dict['type']] 108 | except KeyError: 109 | raise KeyError('ROS2 service type: {} not included in service module: {}'.format(type_dict['type'], 110 | type_dict['module'])) 111 | except ImportError: 112 | raise ImportError('ROS2 service module: {} does not exist.'.format(type_dict['module'])) 113 | except TypeError: 114 | raise TypeError('ROS2 service type: {} does not exist'.format(type_dict['type'])) 115 | 116 | 117 | def ros_interface_loader_str(ros2_type, interface_type): 118 | """ 119 | Wrapper for the :func:`ros_msg_loader` to treat string type command line arguments. 120 | 121 | :param interface_type: A string representing the interface type (message, service,...) 122 | :param ros2_type: A string type ROS2 interface type (e.g. "rosgraph_msgs/Log"). 123 | :return: The :func:`ros_srv_loader` or :func:`ros_msg_loader` function. 124 | """ 125 | type_dict = ros_type_to_dict(ros2_type) 126 | if type_dict: 127 | if interface_type == 'service': 128 | return ros_srv_loader(type_dict) 129 | else: 130 | return ros_msg_loader(type_dict) 131 | else: 132 | raise ImportError('Unable to find defined ROS2 Interface type: {}'.format(ros2_type)) 133 | 134 | 135 | def map_ros_types(ros_class): 136 | """ 137 | A recursive function that maps ROS2 message fields to dictionary 138 | 139 | :param ros_class: The ROS2 class to be fuzzed. 140 | :return: dictionary specifying fieldnames and fieldtypes of a message 141 | """ 142 | msg_type_dict = {} 143 | slot_dict = ros_class.get_fields_and_field_types() 144 | for fieldname, fieldtype in slot_dict.items(): 145 | # print("[*] fieldname, fieldtype:", fieldname, fieldtype) 146 | type_dict = ros_type_to_dict(fieldtype) 147 | # print("[*] type_dict:", type_dict) 148 | if type_dict: 149 | if not type_dict['complex']: 150 | # print("not complex") 151 | if type_dict['array'] or type_dict['sequence']: 152 | parse_basic_arrays(fieldname, type_dict, msg_type_dict) 153 | elif type_dict['type'] == 'string': 154 | msg_type_dict[fieldname] = np.dtype("str") 155 | elif type_dict['type'] == 'boolean': 156 | msg_type_dict[fieldname] = np.dtype("bool") 157 | elif type_dict['type'] == 'octet': 158 | msg_type_dict[fieldname] = np.dtype("byte") 159 | else: # numpy compatible ROS built-in types 160 | msg_type_dict[fieldname] = np.dtype(type_dict['type']) 161 | else: 162 | # print("complex") 163 | parse_complex_types(fieldname, type_dict, msg_type_dict) 164 | 165 | return msg_type_dict 166 | 167 | 168 | def parse_basic_arrays(fieldname, type_dict, msg_type_dict): 169 | """ 170 | Generate Hypothesis strategies for array types. 171 | 172 | :param fieldname: Slot name to be parsed. 173 | :param type_dict: A dictionary which values say if the ROS2 message type is complex (not basic), which is its parent 174 | ROS2 message module, its type, if it is an array and if so, its size. 175 | :param msg_type_dict: A pointer to a dictionary to be filled with Hypothesis strategies. 176 | """ 177 | if type_dict['array_size']: 178 | array_size = int(type_dict['array_size']) 179 | else: 180 | array_size = [] 181 | 182 | if type_dict['type'] == 'string': 183 | msg_type_dict[fieldname] = np.empty(shape=array_size, 184 | dtype=np.dtype("str")) 185 | elif type_dict['type'] == 'boolean': 186 | msg_type_dict[fieldname] = np.empty(shape=array_size, 187 | dtype=np.dtype("bool")) 188 | elif type_dict['type'] == 'octet': 189 | msg_type_dict[fieldname] = np.empty(shape=array_size, 190 | dtype=np.dtype("byte")) 191 | else: 192 | msg_type_dict[fieldname] = np.empty(shape=array_size, 193 | dtype=np.dtype(type_dict["type"])) 194 | 195 | 196 | def parse_complex_types(fieldname, type_dict, msg_type_dict): 197 | """ 198 | Generate Hypothesis strategies for complex ROS2 types. 199 | 200 | :param fieldname: Slot name to be parsed. 201 | :param type_dict: A dictionary which values say if the ROS2 message type is complex (not basic), which is its parent 202 | ROS2 message module, its type, if it is an array or sequence and if so, its size. 203 | :param msg_type_dict: A pointer to a dictionary to be filled with Hypothesis strategies. 204 | """ 205 | if not type_dict['array'] and not type_dict['sequence']: 206 | msg_type_dict[fieldname] = map_ros_types(ros_msg_loader(type_dict)) 207 | else: 208 | # will later modify this to handle arrays of complex types! 209 | if type_dict['array_size']: 210 | msg_type_dict[fieldname] = np.array(map_ros_types(ros_msg_loader(type_dict))) 211 | else: 212 | # msg_type_dict[fieldname] = array(elements=map_ros_types(ros_msg_loader(type_dict))) 213 | # mod: decouple strategy 214 | msg_type_dict[fieldname] = np.array(map_ros_types(ros_msg_loader(type_dict))) 215 | 216 | 217 | # A better approach. It returns an instance of a ROS2 msg directly, so no need for mapping! :) 218 | def dynamic_strategy_generator_ros(draw, type_name, msg_type_dict): # This generates existing ROS msgs objects 219 | aux_obj = type_name() 220 | for key, value in msg_type_dict.items(): 221 | value = draw(value) 222 | # If it is array of numpy, get array python basic type 223 | if isinstance(value, list) and value and hasattr(value[0], 'dtype'): 224 | value = np.array(value) 225 | value = value.tolist() 226 | elif hasattr(value, 'dtype'): 227 | value = value.item() 228 | setattr(aux_obj, key, value) 229 | return aux_obj 230 | -------------------------------------------------------------------------------- /src/ros2_fuzzer/ros_fuzzer.py: -------------------------------------------------------------------------------- 1 | """ 2 | ROS2 Fuzzer CLI interface main module. 3 | 4 | :authors: Alias Robotics S.L. Borja Erice, Odei Olalde, Xabi Perez, Gorka Olalde 5 | """ 6 | import logging 7 | from argparse import ArgumentParser 8 | from hypothesis import given, settings, Verbosity, HealthCheck 9 | from ros2_fuzzer.ros_commons import ros_interface_loader_str, map_ros_types, ROS2NodeFuzzer 10 | 11 | 12 | def fuzz(interface_type, name, ros2_type): 13 | if interface_type == 'message': 14 | fuzz_message_wrapper(ros2_type, name) 15 | elif interface_type == 'service': 16 | fuzz_service_wrapper(ros2_type, name) 17 | else: 18 | raise ValueError(f'Interface type: {interface_type} not supported.') 19 | 20 | 21 | def fuzz_message_wrapper(msg_type, topic_name): 22 | """ 23 | Wrapper for the :func:`fuzz_message` method 24 | :param msg_type: The ROS2 message type 25 | :param topic_name: The name of the topic to be fuzzed 26 | """ 27 | @settings(max_examples=100, verbosity=Verbosity.verbose) 28 | @given(msg=map_ros_types(msg_type)) 29 | def fuzz_message(msg): 30 | fuzzer.publish(msg_type, msg, topic_name) 31 | 32 | with ROS2NodeFuzzer() as fuzzer: 33 | fuzz_message() 34 | 35 | 36 | def fuzz_service_wrapper(srv_type, srv_name): 37 | """ 38 | Wrapper for the :func:`fuzz_service` method 39 | :param srv_type: The ROS2 service type 40 | :param srv_name: The name of the service to be fuzzed 41 | """ 42 | @settings(max_examples=100, verbosity=Verbosity.verbose, suppress_health_check=[HealthCheck.too_slow]) 43 | @given(srv_request=map_ros_types(srv_type.Request)) 44 | def fuzz_service(srv_request): 45 | fuzzer.send_request(srv_type, srv_name, srv_request) 46 | 47 | with ROS2NodeFuzzer() as fuzzer: 48 | fuzz_service() 49 | 50 | 51 | def main(): 52 | """ 53 | Main method. Takes the ros2_interface, ros2_interface_type and name of the topic or service to be fuzzed 54 | as command line arguments and launches the function :func:`fuzz` 55 | """ 56 | logging.basicConfig() 57 | logger = logging.getLogger(__name__) 58 | parser = ArgumentParser(description='ROS2 Fuzzer') 59 | subparsers = parser.add_subparsers(dest='interface_type', help='ROS2 interface type') 60 | subparsers.required = True 61 | 62 | message_parser = subparsers.add_parser('message') 63 | message_parser.add_argument('ros2_type', help='ROS2 message type') 64 | message_parser.add_argument('name', help='The name of the topic to be fuzzed') 65 | 66 | service_parser = subparsers.add_parser('service') 67 | service_parser.add_argument('ros2_type', help='ROS2 service type') 68 | service_parser.add_argument('name', help='The name of the service to be fuzzed') 69 | 70 | args = parser.parse_args() 71 | 72 | try: 73 | ros2_type = ros_interface_loader_str(args.ros2_type, args.interface_type) 74 | fuzz(args.interface_type, args.name, ros2_type) 75 | except Exception as e: 76 | logger.critical('Exception occurred during execution --> ' + str(e)) 77 | 78 | 79 | if __name__ == '__main__': 80 | main() 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | -------------------------------------------------------------------------------- /src/ros2_fuzzer/test.py: -------------------------------------------------------------------------------- 1 | """ 2 | ROS Fuzzer example test cases module. 3 | 4 | :authors: Alias Robotics S.L. Borja Erice, Odei Olalde, Xabi Perez, Gorka Olalde 5 | """ 6 | import unittest 7 | 8 | import rclpy 9 | from hypothesis import given 10 | from rcl_interfaces.msg import ParameterEvent 11 | from rclpy.node import Node 12 | from std_msgs.msg import String 13 | 14 | from ros2_fuzzer.process_handling import FuzzedNodeHandler 15 | from ros2_fuzzer.ros_commons import map_ros_types 16 | 17 | 18 | class TestRosListener(unittest.TestCase): 19 | 20 | def setUp(self): 21 | rclpy.init() 22 | self.node = Node('fuzzer_node') 23 | self.pub = self.node.create_publisher(ParameterEvent, '/chatter') 24 | self.health_checker = FuzzedNodeHandler('/listener') 25 | 26 | def tearDown(self): 27 | rclpy.shutdown('Shutting down fuzzer node') 28 | 29 | @given(message=map_ros_types(String)) 30 | def test_message(self, message): 31 | self.pub.publish(message) 32 | assert self.health_checker.check_if_alive() 33 | 34 | 35 | -------------------------------------------------------------------------------- /src/ros_to_mav.py: -------------------------------------------------------------------------------- 1 | import time 2 | import sys 3 | import os 4 | 5 | # force MAVLink 2.0 6 | os.environ["MAVLINK20"] = "1" 7 | 8 | # doc: https://mavlink.io/en/mavgen_python/ 9 | from pymavlink import mavutil 10 | 11 | # Create a function to send RC values 12 | # More information about Joystick channels 13 | # here: https://www.ardusub.com/operators-manual/rc-input-and-output.html#rc-inputs 14 | def set_rc_channel_pwm(channel_id, pwm=1500): 15 | """ Set RC channel pwm value 16 | Args: 17 | channel_id (TYPE): Channel ID 18 | pwm (int, optional): Channel pwm value 1100-1900 19 | """ 20 | 21 | """ 22 | 1 pitch 23 | 2 roll 24 | 3 throttle 25 | 4 yaw 26 | 5 forward 27 | 6 lateral 28 | 7 camera pan 29 | 8 camera tilt 30 | 9 lights 1 level 31 | 10 lights 2 level 32 | 11 video switch 33 | 34 | [RC Mode 2] 35 | ^ throttle ^ pitch 36 | < > yaw < > roll 37 | v v 38 | 39 | """ 40 | 41 | if channel_id < 1 or channel_id > 18: 42 | print("Channel does not exist.") 43 | return 44 | 45 | # Mavlink 2 supports up to 18 channels: 46 | # https://mavlink.io/en/messages/common.html#RC_CHANNELS_OVERRIDE 47 | rc_channel_values = [65535 for _ in range(18)] 48 | rc_channel_values[channel_id - 1] = pwm 49 | master.mav.rc_channels_override_send( 50 | master.target_system, # target_system 51 | master.target_component, # target_component 52 | *rc_channel_values) # RC channel list, in microseconds. 53 | 54 | 55 | if __name__ == "__main__": 56 | 57 | master = mavutil.mavlink_connection("udpin:127.0.0.1:14550") 58 | 59 | # make sure the connection is valid 60 | master.wait_heartbeat() 61 | print("Heartbeat from system (system %u component %u)" % 62 | (master.target_system, master.target_component)) 63 | 64 | print(master.__dict__) 65 | print("-----") 66 | 67 | # set / connect (virtual) RC before arming to prevent px4 from 68 | # engaging the failsafe mode right away 69 | 70 | master.mav.command_long_send( 71 | master.target_system, 72 | master.target_component, 73 | mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, 74 | 0, 75 | 1, 0, 0, 0, 0, 0, 0 76 | ) 77 | 78 | print("waiting for the vehicle to arm") 79 | master.motors_armed_wait() 80 | print("armed!") 81 | 82 | # ack = False 83 | # while not ack: 84 | # # Wait for ACK command 85 | # ack_msg = master.recv_match(type='COMMAND_ACK', blocking=True) 86 | # ack_msg = ack_msg.to_dict() 87 | 88 | # print(mavutil.mavlink.enums['MAV_RESULT'][ack_msg['result']].description) 89 | # break 90 | 91 | time.sleep(1) 92 | 93 | # # Request all parameters 94 | # master.mav.param_request_list_send( 95 | # master.target_system, master.target_component 96 | # ) 97 | # while True: 98 | # # time.sleep(0.01) 99 | # try: 100 | # message = master.recv_match(type='PARAM_VALUE', blocking=True).to_dict() 101 | # print('name: {}\tvalue: {}'.format(message['param_id'], 102 | # message['param_value'])) 103 | # except Exception as error: 104 | # print(error) 105 | # sys.exit(0) 106 | 107 | # print("end") 108 | 109 | # # px4 verification 110 | # mav_type = master.sysid_state[master.sysid].mav_type 111 | # mav_autopilot = master.sysid_state[master.sysid].mav_autopilot 112 | # print(mav_autopilot == mavutil.mavlink.MAV_AUTOPILOT_PX4) 113 | 114 | """ 115 | {'MANUAL': (81, 1, 0), 'STABILIZED': (81, 7, 0), 'ACRO': (65, 5, 0), 'RATTITUDE': (65, 8, 0), 'ALTCTL': (81, 2, 0), 'POSCTL': (81, 3, 0), 'LOITER': (29, 4, 3), 'MISSION': (29, 4, 4), 'RTL': (29, 4, 5), 'LAND': (29, 4, 6), 'RTGS': (29, 4, 7), 'FOLLOWME': (29, 4, 8), 'OFFBOARD': (29, 6, 0), 'TAKEOFF': (29, 4, 2)} 116 | """ 117 | 118 | mode_str = "MANUAL" 119 | (mode, custom_mode, custom_sub_mode) = master.mode_mapping()[mode_str] 120 | 121 | master.set_mode(mode, custom_mode, custom_sub_mode) 122 | 123 | while True: 124 | # Wait for ACK command 125 | ack_msg = master.recv_match(type='COMMAND_ACK', blocking=True) 126 | ack_msg = ack_msg.to_dict() 127 | print("mode ack:", ack_msg) 128 | 129 | # Check if command in the same in `set_mode` 130 | if ack_msg['command'] != mavutil.mavlink.MAV_CMD_DO_SET_MODE: 131 | continue 132 | 133 | # Print the ACK result ! 134 | print(mavutil.mavlink.enums['MAV_RESULT'][ack_msg['result']].description) 135 | break 136 | 137 | print(f"mode set to {mode_str}") 138 | 139 | time.sleep(5) 140 | 141 | for i in range(1000): 142 | master.mav.manual_control_send( 143 | master.target_system, 144 | 0, # x 145 | 0,# y 146 | 1000, # z 147 | 0, # r 148 | 0) 149 | time.sleep(0.01) 150 | 151 | for i in range(1000): 152 | master.mav.manual_control_send( 153 | master.target_system, 154 | 0, # x 155 | 0,# y 156 | 50, # z 157 | 0, # r 158 | 0) 159 | time.sleep(0.01) 160 | 161 | # time.sleep(10) 162 | 163 | # set_rc_channel_pwm(3, 1900) 164 | 165 | # # https://mavlink.io/en/messages/common.html#MAV_CMD_NAV_TAKEOFF 166 | # master.mav.command_long_send( 167 | # master.target_system, # target_system 168 | # master.target_component, # target_component 169 | # mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, # command (22) 170 | # 0, # confirmation 171 | # 0, # param1 - pitch (deg) 172 | # 0, # param2 - empty 173 | # 0, # param3 - empty 174 | # 0, # param4 - yaw angle (deg) 175 | # 0, # param5 - lat 176 | # 0, # param6 - lon 177 | # 100) # param7 - altitude (m) 178 | 179 | # ack = False 180 | # while not ack: 181 | # # Wait for ACK command 182 | # ack_msg = master.recv_match(type='COMMAND_ACK', blocking=True) 183 | # ack_msg = ack_msg.to_dict() 184 | # print("takeoff ack:", ack_msg) 185 | 186 | # if ack_msg['command'] != mavutil.mavlink.MAV_CMD_NAV_TAKEOFF: 187 | # continue 188 | 189 | # print(mavutil.mavlink.enums['MAV_RESULT'][ack_msg['result']].description) 190 | # break 191 | 192 | # print("takeoff command acked") 193 | # time.sleep(25) 194 | 195 | while True: 196 | print("blocking") 197 | time.sleep(2) 198 | -------------------------------------------------------------------------------- /src/ros_utils.py: -------------------------------------------------------------------------------- 1 | from collections import defaultdict 2 | import importlib 3 | 4 | from ament_index_python import get_resource, get_resources, has_resource 5 | 6 | 7 | def get_all_message_types(): 8 | all_message_types = {} 9 | for package_name in get_resources("rosidl_interfaces"): 10 | message_types = get_message_types(package_name) 11 | if message_types: 12 | all_message_types[package_name] = message_types 13 | return all_message_types 14 | 15 | 16 | def get_message_types(package_name): 17 | if not has_resource("packages", package_name): 18 | raise LookupError("Unknown package name") 19 | try: 20 | content, _ = get_resource("rosidl_interfaces", package_name) 21 | except LookupError: 22 | return [] 23 | interface_names = content.splitlines() 24 | # TODO(dirk-thomas) this logic should come from a rosidl related package 25 | # Only return messages in msg folder 26 | return list( 27 | sorted( 28 | { 29 | n[4:-4] 30 | for n in interface_names 31 | if n.startswith("msg/") and n[-4:] in (".idl", ".msg") 32 | } 33 | ) 34 | ) 35 | 36 | 37 | def get_all_service_types(): 38 | all_service_types = {} 39 | for package_name in get_resources("rosidl_interfaces"): 40 | service_types = get_service_types(package_name) 41 | if service_types: 42 | all_service_types[package_name] = service_types 43 | return all_service_types 44 | 45 | 46 | def get_service_types(package_name): 47 | if not has_resource("packages", package_name): 48 | raise LookupError("Unknown package name") 49 | try: 50 | content, _ = get_resource("rosidl_interfaces", package_name) 51 | except LookupError: 52 | return [] 53 | interface_names = content.splitlines() 54 | # TODO(dirk-thomas) this logic should come from a rosidl related package 55 | # Only return services in srv folder 56 | return list( 57 | sorted( 58 | { 59 | n[4:-4] 60 | for n in interface_names 61 | if n.startswith("srv/") and n[-4:] in (".idl", ".srv") 62 | } 63 | ) 64 | ) 65 | 66 | 67 | def get_publishers(node): 68 | publishers = defaultdict(list) 69 | 70 | for name, namespace in node.get_node_names_and_namespaces(): 71 | if name == "_fuzzer": 72 | continue 73 | 74 | if namespace.endswith("/"): 75 | node_name = namespace + name 76 | else: 77 | node_name = namespace + "/" + name 78 | 79 | for ( 80 | topic_name, 81 | topic_type, 82 | ) in node.get_publisher_names_and_types_by_node(name, namespace): 83 | publishers[topic_name].append(node_name) 84 | 85 | return publishers 86 | 87 | 88 | def get_subscriptions(node): 89 | subscriptions = defaultdict(list) 90 | # {node_name: [(topic_name, topic_type), (topic_name, topic_type)]} 91 | 92 | for name, namespace in node.get_node_names_and_namespaces(): 93 | if name == "_fuzzer": 94 | continue 95 | 96 | if namespace.endswith("/"): 97 | node_name = namespace + name 98 | else: 99 | node_name = namespace + "/" + name 100 | 101 | for ( 102 | topic_name, 103 | topic_type, 104 | ) in node.get_subscriber_names_and_types_by_node(name, namespace): 105 | # print(node_name, "subs to", topic_name, topic_type) 106 | subscriptions[node_name].append((topic_name, topic_type)) 107 | 108 | return subscriptions 109 | 110 | 111 | def get_secure_subscriptions(node): 112 | subscriptions = defaultdict(list) 113 | # {node_name: [(topic_name, topic_type), (topic_name, topic_type)]} 114 | 115 | for ( 116 | name, 117 | namespace, 118 | enclave, 119 | ) in node.get_node_names_and_namespaces_with_enclaves(): 120 | if name == "_fuzzer": 121 | continue 122 | 123 | if namespace.endswith("/"): 124 | node_name = namespace + name 125 | else: 126 | node_name = namespace + "/" + name 127 | 128 | for ( 129 | topic_name, 130 | topic_type, 131 | ) in node.get_subscriber_names_and_types_by_node(name, namespace): 132 | # print(node_name, "subs to", topic_name, topic_type) 133 | subscriptions[node_name].append((topic_name, topic_type)) 134 | 135 | return subscriptions 136 | 137 | 138 | def get_services(node): 139 | services = defaultdict(list) 140 | 141 | for name, namespace in node.get_node_names_and_namespaces(): 142 | if name == "_fuzzer": 143 | continue 144 | 145 | if namespace.endswith("/"): 146 | node_name = namespace + name 147 | else: 148 | node_name = namespace + "/" + name 149 | 150 | for ( 151 | service_name, 152 | service_type, 153 | ) in node.get_service_names_and_types_by_node(name, namespace): 154 | services[node_name].append((service_name, service_type)) 155 | 156 | return services 157 | 158 | 159 | def find_custom_msg(msg_type): 160 | return None 161 | 162 | 163 | def get_msg_class_from_name(msg_pkg, msg_name): 164 | # e.g., get_msg_class_from_name("geometry_msgs", "Vector3") 165 | msg_type_class = None 166 | try: 167 | module = importlib.import_module(msg_pkg + ".msg") 168 | msg_type_class = module.__dict__[msg_name] 169 | except KeyError: 170 | print("[-] {} not in pkg {}".format(msg_name, msg_pkg)) 171 | except ImportError: 172 | print("[-] pkg {} doesn't exist.".format(msg_pkg)) 173 | except TypeError: 174 | print("[-] {} doesn't exist".format(msg_name)) 175 | 176 | return msg_type_class 177 | 178 | 179 | def get_msg_typename_from_class(msg_type_class): 180 | module = msg_type_class.__dict__["__module__"].split(".")[0] 181 | 182 | # mtc is a metaclass, and its instance is class 183 | class_name = msg_type_class().__class__.__name__ 184 | 185 | typename = f"{module}/{class_name}" 186 | 187 | return typename 188 | 189 | 190 | def flatten_nested_dict(indict, pre=None): 191 | pre = pre[:] if pre else [] 192 | if isinstance(indict, dict): 193 | for key, value in indict.items(): 194 | if isinstance(value, dict): 195 | for d in flatten_nested_dict(value, pre + [key]): 196 | yield d 197 | elif isinstance(value, list) or isinstance(value, tuple): 198 | for v in value: 199 | for d in flatten_nested_dict(v, pre + [key]): 200 | yield d 201 | else: 202 | yield pre + [key, value] 203 | else: 204 | yield pre + [indict] 205 | -------------------------------------------------------------------------------- /src/rosbag_parser.py: -------------------------------------------------------------------------------- 1 | import math 2 | import sqlite3 3 | from rclpy.serialization import deserialize_message 4 | from rosidl_runtime_py.utilities import get_message 5 | 6 | def trim_ts(ts, digits=16): 7 | if len(str(ts)) < digits: 8 | ts = int(str(ts).zfill(digits)) 9 | else: 10 | ts = int(str(ts)[:digits]) 11 | 12 | return ts 13 | 14 | 15 | class RosbagParser: 16 | flag_topic_name = "/listen_flag" 17 | debug = False 18 | 19 | def __init__(self, db_file): 20 | self.conn, self.cursor = self.connect(db_file) 21 | 22 | topics = self.get_all_topics() 23 | 24 | self.topic_name_type_map = { 25 | topic_name: topic_type 26 | for topic_id, topic_name, topic_type, _, _ in topics 27 | } 28 | 29 | self.topic_id_name_map = { 30 | topic_id: topic_name 31 | for topic_id, topic_name, topic_type, _, _ in topics 32 | } 33 | 34 | self.topic_name_msg_map = { 35 | topic_name: get_message(topic_type) 36 | for topic_id, topic_name, topic_type, _, _ in topics 37 | } 38 | 39 | self.messages = self.get_all_messages() 40 | 41 | found = self.find_flag_info() 42 | if not found: 43 | self.abort = True 44 | else: 45 | self.abort = False 46 | 47 | def connect(self, db_file): 48 | """Make connection to an SQLite database file.""" 49 | 50 | conn = sqlite3.connect(db_file) 51 | c = conn.cursor() 52 | 53 | return conn, c 54 | 55 | def get_all_topics(self): 56 | """get all topic names and types""" 57 | 58 | self.cursor.execute("SELECT * from topics;") 59 | 60 | rows = self.cursor.fetchall() 61 | 62 | if self.debug: 63 | for row in rows: 64 | print(row) 65 | 66 | return rows 67 | 68 | def get_all_messages(self): 69 | 70 | self.cursor.execute("SELECT * from messages;") 71 | 72 | rows = self.cursor.fetchall() 73 | 74 | if self.debug: 75 | for row in rows: 76 | print(row) 77 | 78 | return rows 79 | 80 | def find_flag_info(self): 81 | """Find listen_flag topic id""" 82 | return True 83 | 84 | found = False 85 | for topic_id, topic_name in self.topic_id_name_map.items(): 86 | if topic_name == self.flag_topic_name: 87 | self.flag_topic_id = topic_id 88 | found = True 89 | 90 | if not found: 91 | print("[-] listen_flag message is dropped") 92 | 93 | return found 94 | 95 | def process_messages(self) -> dict: 96 | """ 97 | Retrieve messages published only when listen_flag is activated, 98 | and deserialize the CDR-formatted data. 99 | """ 100 | 101 | filtered_messages_by_topic = dict() 102 | 103 | """ 104 | start_ts = 0 105 | end_ts = 0 106 | listen_flag = False 107 | for msg_id, topic_id, ts, data in self.messages: 108 | if topic_id == self.flag_topic_id: 109 | flag_data = deserialize_message( 110 | data, self.topic_name_msg_map[self.flag_topic_name] 111 | ) 112 | 113 | if listen_flag is False: 114 | start_ts = flag_data.data 115 | listen_flag = True 116 | else: 117 | end_ts = flag_data.data 118 | listen_flag = False 119 | 120 | break 121 | 122 | """ 123 | 124 | with open("/tmp/start_ts", "r") as f: 125 | start_ts = int(f.readline()) 126 | with open("/tmp/end_ts", "r") as f: 127 | end_ts = int(f.readline()) 128 | 129 | # trim/extend digits for comparison 130 | digits = 16 # 10 + 6 (e.g., 1641420015.780356) 131 | start_ts = trim_ts(start_ts, digits) 132 | end_ts = trim_ts(end_ts, digits) 133 | 134 | # print(start_ts, end_ts) 135 | 136 | for msg_id, topic_id, ts, data in self.messages: 137 | # if topic_id != self.flag_topic_id: 138 | ts_trimmed = trim_ts(ts, digits) 139 | if ts_trimmed > start_ts and ts_trimmed < end_ts: 140 | topic_name = self.topic_id_name_map[topic_id] 141 | deserialized_data = deserialize_message( 142 | data, self.topic_name_msg_map[topic_name] 143 | ) 144 | if topic_name not in filtered_messages_by_topic: 145 | filtered_messages_by_topic[topic_name] = [ 146 | (ts, deserialized_data) 147 | ] 148 | else: 149 | filtered_messages_by_topic[topic_name].append( 150 | (ts, deserialized_data) 151 | ) 152 | 153 | return filtered_messages_by_topic 154 | 155 | def process_all_messages(self) -> dict: 156 | """ 157 | Retrieve all messages recorded in the rosbag 158 | and deserialize the CDR-formatted data. 159 | """ 160 | 161 | filtered_messages_by_topic = dict() 162 | 163 | for msg_id, topic_id, ts, data in self.messages: 164 | topic_name = self.topic_id_name_map[topic_id] 165 | deserialized_data = deserialize_message( 166 | data, self.topic_name_msg_map[topic_name] 167 | ) 168 | if topic_name not in filtered_messages_by_topic: 169 | filtered_messages_by_topic[topic_name] = [ 170 | (ts, deserialized_data) 171 | ] 172 | else: 173 | filtered_messages_by_topic[topic_name].append( 174 | (ts, deserialized_data) 175 | ) 176 | 177 | return filtered_messages_by_topic 178 | 179 | 180 | if __name__ == "__main__": 181 | # test 182 | db3_file = "states-0.bag/states-0.bag_0.db3" 183 | parser = RosbagParser(db3_file) 184 | msgs = parser.process_all_messages() 185 | print(msgs.keys()) 186 | 187 | joint_states = msgs["/joint_states"] 188 | panda_arm_controller_state = msgs["/panda_arm_controller/state"] 189 | move_action_status = msgs["/move_action/_action/status"] 190 | motion_plan_requests = msgs["/motion_plan_request"] 191 | 192 | for (ts, cont_state) in panda_arm_controller_state: 193 | print(cont_state.error.positions) 194 | print(len(cont_state.error.positions)) 195 | 196 | -------------------------------------------------------------------------------- /src/state_monitor.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import os 3 | import importlib 4 | import time 5 | import pickle 6 | import json 7 | import threading 8 | import logging 9 | 10 | import constants as c 11 | 12 | import rclpy 13 | from rclpy.node import Node 14 | 15 | from std_msgs.msg import Bool 16 | from std_srvs.srv import Empty 17 | 18 | 19 | import ros_utils 20 | 21 | 22 | class StateMonitorNode(Node): 23 | msg_queue = list() 24 | subs = dict() 25 | do_record = False 26 | is_recording = False 27 | 28 | def __init__(self, watchlist): 29 | super().__init__("_state_monitor") 30 | 31 | self.watchlist = watchlist 32 | 33 | # self.srv_start_monitor = self.create_service(Empty, "start_monitor", 34 | # self.start_monitor_callback) 35 | 36 | # self.srv_stop_monitor = self.create_service(Empty, "stop_monitor", 37 | # self.stop_monitor_callback) 38 | 39 | self.logger = self.init_logger() 40 | 41 | # self.listen_flag = self.create_subscription( 42 | # Bool, 43 | # "_listen_flag", 44 | # self.listen_callback, 45 | # 10 46 | # ) 47 | 48 | # re-create subscriptions every time monitoring is requested 49 | for target in self.watchlist: 50 | topic_msg_type = self.watchlist[target] 51 | pkg_name = ".".join(topic_msg_type.split("/")[:-1]) 52 | module_name = topic_msg_type.split("/")[-1] 53 | 54 | msg_type_class = ros_utils.get_msg_class_from_name( 55 | pkg_name.split(".")[0], module_name) 56 | 57 | self.subs[target] = self.create_subscription( 58 | msg_type_class, target, self.msg_callback, 10) 59 | print(f"[state monitor] subscribed to {target}") 60 | self.logger.debug(f"subscribed to {target}") 61 | 62 | def init_logger(self): 63 | logger = logging.getLogger("state_monitor") 64 | logger.setLevel(logging.DEBUG) 65 | 66 | fh = logging.FileHandler("state_monitor.log") 67 | fh.setLevel(logging.DEBUG) 68 | fmt = logging.Formatter('%(asctime)s - %(name)s - %(levelname)s - %(message)s') 69 | fh.setFormatter(fmt) 70 | 71 | logger.addHandler(fh) 72 | 73 | return logger 74 | 75 | # def start_monitor_callback(self, request, response): 76 | # print("[state monitor] start_monitor service called") 77 | # self.logger.debug("start_monitor service called") 78 | 79 | # self.do_record = True 80 | 81 | # self.logger.debug(f"returning from start_monitor_callback") 82 | # return response 83 | 84 | # def stop_monitor_callback(self, request, response): 85 | # print("[state monitor] stop_monitor service called") 86 | # self.logger.debug("stop_monitor service called") 87 | # self.do_record = False 88 | 89 | # # print(len(self.msg_queue)) 90 | 91 | # # self.is_recording = False 92 | # self.dump_to_file() 93 | 94 | # self.msg_queue = list() 95 | 96 | # # # destroy subscriptions 97 | # # for target in self.subs: 98 | # # sub = self.subs[target] 99 | # # print(sub) 100 | # # sub.destroy() 101 | # # print(f"[state monitor] unsubscribed from {target}") 102 | # # self.subs = dict() 103 | 104 | # # # destroy subscriptions 105 | # # for target in self.subs: 106 | # # res = self.destroy_subscription(self.subs[target]) 107 | # # assert(res is True) 108 | # # print(f"[state monitor] unsubscribed from {target}") 109 | # # self.logger.debug(f"unsubscribed from {target}") 110 | 111 | # try: 112 | # os.remove(c.PUBLOCK) 113 | # except: 114 | # pass 115 | 116 | # print("[state monitor] stop_monitor_callback returning") 117 | # self.logger.debug("stop_monitor_callback returning") 118 | # return response 119 | 120 | def msg_callback(self, msg): 121 | # These callbacks are not being called persistently as expected; 122 | # when start_monitor request is received, the node is supposed 123 | # to be already listening to the watchlist topic, and calling 124 | # this callback. However, this callback function is called a while 125 | # after the request is handled. 126 | # Tried many different things to fix this, but nothing works. 127 | # Will move on by just stalling the executor from publishing messages 128 | # until this callback is called by placing a barrier 129 | # so that no state is lost after messages are published :( 130 | 131 | self.logger.debug("callback", self.is_recording) 132 | if not os.path.isfile(c.PUBLOCK): 133 | self.logger.debug("creating publock file") 134 | fp = open(c.PUBLOCK, "w") 135 | fp.close() 136 | 137 | 138 | if not os.path.isfile(c.WATCHFLAG): 139 | if self.is_recording: 140 | self.is_recording = False 141 | self.logger.debug("dumping states to file") 142 | self.dump_to_file() 143 | self.msg_queue = list() 144 | 145 | try: 146 | os.remove(c.PUBLOCK) 147 | except: 148 | pass 149 | 150 | else: 151 | self.is_recording = True 152 | 153 | self.logger.debug("enqueueing msgs") 154 | self.msg_queue.append(msg) 155 | 156 | # if self.do_record: 157 | # self.msg_queue.append(msg) 158 | 159 | def listen_callback(self, msg): 160 | if msg.data is True: 161 | self.do_record = True 162 | self.is_recording = True 163 | print("start recording") 164 | else: 165 | self.do_record = False 166 | if self.is_recording: 167 | print("stop recording") 168 | print(len(self.msg_queue)) 169 | self.is_recording = False 170 | self.dump_to_file() 171 | self.msg_queue = list() 172 | 173 | def dump_to_file(self): 174 | pickle.dump(self.msg_queue, open("states.pkl", "wb")) 175 | 176 | 177 | def main(watchlist_file): 178 | rclpy.init() 179 | 180 | with open(watchlist_file, "r") as fp: 181 | data = fp.read() 182 | 183 | watchlist = json.loads(data) 184 | 185 | _sm = StateMonitorNode(watchlist) 186 | node_executor = rclpy.executors.MultiThreadedExecutor(12) 187 | node_executor.add_node(_sm) 188 | node_executor_thread = threading.Thread( 189 | target=node_executor.spin, 190 | daemon=True 191 | ) 192 | # rclpy.spin(_sm) 193 | node_executor_thread.start() 194 | 195 | rate = _sm.create_rate(10) # 2 Hz 196 | try: 197 | while rclpy.ok(): 198 | # print("[state monitor] listening, {}".format(_sm.do_record)) 199 | rate.sleep() 200 | except KeyboardInterrupt: 201 | print("[state monitor] aborting as user requested") 202 | 203 | _sm.destroy_node() 204 | rclpy.shutdown() 205 | node_executor_thread.join() 206 | 207 | 208 | if __name__ == "__main__": 209 | if len(sys.argv) < 2: 210 | print("Usage: $ python3 {} [watchlist_file]".format(sys.argv[0])) 211 | sys.exit(1) 212 | 213 | watchlist_file = sys.argv[1].strip() 214 | if not os.path.isfile(watchlist_file): 215 | print("Cannot find {}".format(watchlist_file)) 216 | sys.exit(1) 217 | 218 | try: 219 | os.remove("states.pkl") 220 | except: 221 | pass 222 | 223 | try: 224 | os.remove(c.PUBLOCK) 225 | except: 226 | pass 227 | 228 | try: 229 | os.remove(c.WATCHFLAG) 230 | except: 231 | pass 232 | 233 | main(watchlist_file) 234 | 235 | -------------------------------------------------------------------------------- /src/tests/test_arith_float.py: -------------------------------------------------------------------------------- 1 | import struct 2 | import sys 3 | import numpy as np 4 | 5 | sys.path.append("../") 6 | import mutator as m 7 | 8 | 9 | dtype = np.dtype("float32") 10 | v = m.gen_rand_data(dtype) 11 | 12 | print("original data") 13 | print(v) 14 | print(m.float32_to_bitstr(v)) 15 | 16 | for i in range(0, 32, 8): 17 | mutv = m.mutate_one(dtype, v, m.STAGE_ARITH8, i, 35) 18 | if mutv is not None: 19 | print(m.float32_to_bitstr(v), v) 20 | print("="*i + "v"*8 + "="*(24-i)) 21 | print(m.float32_to_bitstr(mutv), mutv) 22 | print("") 23 | else: 24 | print("skip mutation") 25 | 26 | for i in range(0, 32, 8): 27 | mutv = m.mutate_one(dtype, v, m.STAGE_ARITH16, i, 1) 28 | if mutv is not None: 29 | print(m.float32_to_bitstr(v), v) 30 | print("="*i + "v"*16 + "="*(16-i)) 31 | print(m.float32_to_bitstr(mutv), mutv) 32 | print("") 33 | else: 34 | print("skip mutation") 35 | 36 | 37 | dtype = np.dtype("float64") 38 | v = m.gen_rand_data(dtype) 39 | 40 | print("original data") 41 | print(v) 42 | print(m.float64_to_bitstr(v)) 43 | 44 | for i in range(0, 64, 8): 45 | mutv = m.mutate_one(dtype, v, m.STAGE_ARITH32, i, 35) 46 | if mutv is not None: 47 | print(m.float64_to_bitstr(v), v) 48 | print("="*i + "v"*32 + "="*(32-i)) 49 | print(m.float64_to_bitstr(mutv), mutv) 50 | print("") 51 | else: 52 | print("skip mutation") 53 | 54 | -------------------------------------------------------------------------------- /src/tests/test_arith_int.py: -------------------------------------------------------------------------------- 1 | import struct 2 | import sys 3 | import numpy as np 4 | 5 | sys.path.append("../") 6 | import mutator as m 7 | 8 | 9 | dtype = np.dtype("int16") 10 | v = m.gen_rand_data(dtype) 11 | 12 | print("original data") 13 | print(v) 14 | print(m.int16_to_bitstr(v)) 15 | 16 | for i in range(0, 16, 8): 17 | mutv = m.mutate_one(dtype, v, m.STAGE_ARITH8, i, 35) 18 | if mutv is not None: 19 | print(m.int16_to_bitstr(v), v) 20 | print(m.int16_to_bitstr(mutv), mutv) 21 | print("") 22 | 23 | for i in range(0, 16, 8): 24 | mutv = m.mutate_one(dtype, v, m.STAGE_ARITH16, i, 1) 25 | if mutv is not None: 26 | print(m.int16_to_bitstr(v), v) 27 | print(m.int16_to_bitstr(mutv), mutv) 28 | print("") 29 | 30 | dtype = np.dtype("int32") 31 | v = m.gen_rand_data(dtype) 32 | 33 | print("original data") 34 | print(v) 35 | print(m.int_to_bitstr(32, v)) 36 | 37 | for i in range(0, 32, 8): 38 | mutv = m.mutate_one(dtype, v, m.STAGE_ARITH8, i, 35) 39 | if mutv is not None: 40 | print(m.int32_to_bitstr(v), v) 41 | print("="*i + "v"*8 + "="*(24-i)) 42 | print(m.int32_to_bitstr(mutv), mutv) 43 | print("") 44 | else: 45 | print("skip mutation") 46 | 47 | for i in range(0, 32, 8): 48 | mutv = m.mutate_one(dtype, v, m.STAGE_ARITH16, i, 1) 49 | if mutv is not None: 50 | print(m.int32_to_bitstr(v), v) 51 | print("="*i + "v"*16 + "="*(16-i)) 52 | print(m.int32_to_bitstr(mutv), mutv) 53 | print("") 54 | else: 55 | print("skip mutation") 56 | 57 | 58 | dtype = np.dtype("int64") 59 | v = m.gen_rand_data(dtype) 60 | 61 | print("original data") 62 | print(v) 63 | print(m.int_to_bitstr(64, v)) 64 | 65 | for i in range(0, 64, 8): 66 | mutv = m.mutate_one(dtype, v, m.STAGE_ARITH32, i, 35) 67 | if mutv is not None: 68 | print(m.int64_to_bitstr(v), v) 69 | print("="*i + "v"*32 + "="*(32-i)) 70 | print(m.int64_to_bitstr(mutv), mutv) 71 | print("") 72 | else: 73 | print("skip mutation") 74 | 75 | # for i in range(32): 76 | # mutv = mutator.mutate_one(dtype, v, mutator.STAGE_FLIP8, i) 77 | # print("="*(i) + "^"*8 + "="*(32-i-8)) 78 | # print(mutator.float32_to_bitstr(v)) 79 | # print(mutator.float32_to_bitstr(mutv), mutv) 80 | # print("") 81 | 82 | 83 | # v = mutator.rand_float64() 84 | # print(v) 85 | # print(mutator.float64_to_bitstr(v)) 86 | 87 | # dtype = numpy.dtype("float64") 88 | # for i in range(64): 89 | # mutv = mutator.mutate_one(dtype, v, mutator.STAGE_FLIP1, i) 90 | # print("="*(i) + "^" + "="*(64-i-1)) 91 | # print(mutator.float64_to_bitstr(v)) 92 | # print(mutator.float64_to_bitstr(mutv), mutv) 93 | # print("") 94 | 95 | # for i in range(64): 96 | # mutv = mutator.mutate_one(dtype, v, mutator.STAGE_FLIP2, i) 97 | # print("="*(i) + "^"*2 + "="*(64-i-2)) 98 | # print(mutator.float64_to_bitstr(v)) 99 | # print(mutator.float64_to_bitstr(mutv), mutv) 100 | # print("") 101 | -------------------------------------------------------------------------------- /src/tests/test_flips_bool.py: -------------------------------------------------------------------------------- 1 | import struct 2 | import sys 3 | import numpy as np 4 | 5 | sys.path.append("../") 6 | import mutator as m 7 | 8 | 9 | stage = m.STAGE_FLIP1 10 | dtype = np.dtype("bool") 11 | data_val = True 12 | for i in range(8): 13 | mutated = m.mutate_one(dtype, data_val, stage, i) 14 | if mutated is not None: 15 | print(mutated, bin(mutated)) 16 | 17 | stage = m.STAGE_FLIP2 18 | for i in range(8): 19 | mutated = m.mutate_one(dtype, data_val, stage, i) 20 | if mutated is not None: 21 | print(mutated, bin(mutated)) 22 | 23 | -------------------------------------------------------------------------------- /src/tests/test_flips_byte.py: -------------------------------------------------------------------------------- 1 | import struct 2 | import sys 3 | import numpy as np 4 | 5 | sys.path.append("../") 6 | import mutator as m 7 | 8 | stage = m.STAGE_FLIP1 9 | dtype = np.dtype("byte") 10 | data_val = 0xff 11 | for i in range(8): 12 | mutated = m.mutate_one(dtype, data_val, stage, i) 13 | if mutated is not None: 14 | print(mutated, bin(mutated)) 15 | 16 | stage = m.STAGE_FLIP2 17 | for i in range(8): 18 | mutated = m.mutate_one(dtype, data_val, stage, i) 19 | if mutated is not None: 20 | print(mutated, bin(mutated)) 21 | 22 | -------------------------------------------------------------------------------- /src/tests/test_flips_float.py: -------------------------------------------------------------------------------- 1 | import struct 2 | import sys 3 | import numpy as np 4 | 5 | sys.path.append("../") 6 | import mutator as m 7 | 8 | 9 | v = m.rand_float32() 10 | 11 | print(v) 12 | print(m.float32_to_bitstr(v)) 13 | 14 | dtype = np.dtype("float32") 15 | for i in range(32): 16 | mutv = m.mutate_one(dtype, v, m.STAGE_FLIP1, i) 17 | print("="*(i) + "^" + "="*(32-i-1)) 18 | print(m.float32_to_bitstr(v)) 19 | print(m.float32_to_bitstr(mutv), mutv) 20 | print("") 21 | 22 | for i in range(32): 23 | mutv = m.mutate_one(dtype, v, m.STAGE_FLIP8, i) 24 | print("="*(i) + "^"*8 + "="*(32-i-8)) 25 | print(m.float32_to_bitstr(v)) 26 | print(m.float32_to_bitstr(mutv), mutv) 27 | print("") 28 | 29 | 30 | v = m.rand_float64() 31 | print(v) 32 | print(m.float64_to_bitstr(v)) 33 | 34 | dtype = np.dtype("float64") 35 | for i in range(64): 36 | mutv = m.mutate_one(dtype, v, m.STAGE_FLIP1, i) 37 | print("="*(i) + "^" + "="*(64-i-1)) 38 | print(m.float64_to_bitstr(v)) 39 | print(m.float64_to_bitstr(mutv), mutv) 40 | print("") 41 | 42 | for i in range(64): 43 | mutv = m.mutate_one(dtype, v, m.STAGE_FLIP2, i) 44 | print("="*(i) + "^"*2 + "="*(64-i-2)) 45 | print(m.float64_to_bitstr(v)) 46 | print(m.float64_to_bitstr(mutv), mutv) 47 | print("") 48 | -------------------------------------------------------------------------------- /src/tests/test_flips_int.py: -------------------------------------------------------------------------------- 1 | import struct 2 | import sys 3 | import numpy as np 4 | 5 | sys.path.append("../") 6 | import mutator as m 7 | 8 | 9 | stage = m.STAGE_FLIP1 10 | dtype = np.dtype("int8") 11 | data_val = 10 12 | for i in range(8): 13 | mutated = m.mutate_one(dtype, data_val, stage, i) 14 | print(mutated, bin(mutated)) 15 | 16 | stage = m.STAGE_FLIP2 17 | dtype = np.dtype("int8") 18 | data_val = 10 19 | for i in range(8): 20 | mutated = m.mutate_one(dtype, data_val, stage, i) 21 | print(mutated, bin(mutated)) 22 | 23 | stage = m.STAGE_FLIP4 24 | dtype = np.dtype("int8") 25 | data_val = 10 26 | for i in range(8): 27 | mutated = m.mutate_one(dtype, data_val, stage, i) 28 | print(mutated, bin(mutated)) 29 | 30 | stage = m.STAGE_FLIP8 31 | dtype = np.dtype("int8") 32 | data_val = 10 33 | for i in range(8): 34 | mutated = m.mutate_one(dtype, data_val, stage, i) 35 | print(mutated, bin(mutated)) 36 | -------------------------------------------------------------------------------- /src/tests/test_kinematics.py: -------------------------------------------------------------------------------- 1 | import math 2 | 3 | import kinpy as kp 4 | """ 5 | prerequisite: 6 | $ pip3 install kinpy 7 | """ 8 | 9 | panda_urdf = "/opt/ros/foxy/share/moveit_resources_panda_description/urdf/panda.urdf" 10 | chain = kp.build_chain_from_urdf(open(panda_urdf).read()) 11 | serial_chain = kp.build_serial_chain_from_urdf( 12 | open(panda_urdf).read(), 13 | "panda_hand" 14 | ) 15 | 16 | # print(chain) 17 | # print(chain.get_joint_parameter_names()) 18 | 19 | # print(serial_chain) 20 | # print(serial_chain.get_joint_parameter_names()) 21 | 22 | 23 | # default position 24 | th_default = { 25 | "panda_finger_joint1": math.radians(0), 26 | "panda_joint1": math.radians(0), 27 | "panda_joint2": math.radians(-45), 28 | "panda_finger_joint2": math.radians(0), 29 | "panda_joint3": math.radians(0), 30 | "panda_joint4": math.radians(-135), 31 | "panda_joint5": math.radians(0), 32 | "panda_joint6": math.radians(90), 33 | "panda_joint7": math.radians(45), 34 | } 35 | 36 | # x:=0.28 y:=-0.2 z:=0.3 w:=1.0 37 | """ 38 | $ ros2 topic echo /joint_states 39 | ... 40 | name: 41 | - panda_finger_joint1 42 | - panda_joint1 43 | - panda_joint2 44 | - panda_finger_joint2 45 | - panda_joint3 46 | - panda_joint4 47 | - panda_joint5 48 | - panda_joint6 49 | - panda_joint7 50 | position: 51 | - 0.0 52 | - -1.897337603173183 53 | - -1.7623374174942728 54 | - 0.0 55 | - 2.0174633194485563 56 | - -2.6834497838874456 57 | - -1.0873036202046826 58 | - 1.5543641496734297 59 | - 1.487687515257222 60 | """ 61 | th = { 62 | "panda_finger_joint1": 0.0, 63 | "panda_joint1": -1.897337603173183, 64 | "panda_joint2": -1.7623374174942728, 65 | "panda_finger_joint2": 0.0, 66 | "panda_joint3": 2.0174633194485563, 67 | "panda_joint4": -2.6834497838874456, 68 | "panda_joint5": -1.0873036202046826, 69 | "panda_joint6": 1.5543641496734297, 70 | "panda_joint7": 1.487687515257222 71 | } 72 | 73 | """ 74 | - 0.0 75 | - 3.761173896491529e-05 76 | - 5.496248295528061e-05 77 | - 0.0 78 | - 7.947159116156401e-05 79 | - 5.475179431881512e-06 80 | - -4.8680176725611096e-05 81 | - 1.5709897997769993 82 | - 0.7850355813402683 83 | """ 84 | th_extended = { 85 | "panda_finger_joint1": 0.0, 86 | "panda_joint1": 3.761173896491529e-05, 87 | "panda_joint2": 5.496248295528061e-05, 88 | "panda_finger_joint2": 0.0, 89 | "panda_joint3": 7.947159116156401e-05, 90 | "panda_joint4": 5.475179431881512e-06, 91 | "panda_joint5": -4.8680176725611096e-05, 92 | "panda_joint6": 1.5709897997769993, 93 | "panda_joint7": 0.7850355813402683 94 | } 95 | 96 | 97 | fk = chain.forward_kinematics(th_default) 98 | print(fk["panda_hand"]) 99 | eff_pos = fk["panda_hand"].pos 100 | # print(eff_pos[0], eff_pos[1], eff_pos[2]) 101 | """ 102 | Transform( 103 | rot=[ 9.24016073e-01 2.43041390e-04 -5.21072594e-05 -3.82353548e-01], 104 | pos=[ 0.28003789 -0.20003787 0.30004261] 105 | ) 106 | """ 107 | import numpy as np 108 | import copy 109 | tf_nosol = kp.Transform() 110 | tf_nosol.rot[0] = 0.0 111 | tf_nosol.rot[1] = 0.0 112 | tf_nosol.rot[2] = 0.0 113 | tf_nosol.rot[3] = 1.0 114 | tf_nosol.pos[0] = 20.0 115 | tf_nosol.pos[1] = 0.0 116 | tf_nosol.pos[2] = 0.0 117 | print(tf_nosol) 118 | 119 | import math 120 | ik = serial_chain.inverse_kinematics(tf_nosol) 121 | print(serial_chain.get_joint_parameter_names()) 122 | for joint_angle in ik: 123 | print(math.degrees(joint_angle)) 124 | 125 | -------------------------------------------------------------------------------- /src/tests/test_tracer.py: -------------------------------------------------------------------------------- 1 | from tracer import APITracer 2 | 3 | t = APITracer() 4 | d = t.parse_trace(filename="trace-0") 5 | print(d) 6 | -------------------------------------------------------------------------------- /src/tests/test_unicode.py: -------------------------------------------------------------------------------- 1 | import sys 2 | sys.path.append("../") 3 | 4 | import mutator as m 5 | 6 | s = m.rand_unicode(10) 7 | print("|" + s + "|", len(s), type(s)) 8 | -------------------------------------------------------------------------------- /src/tracer.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | 4 | class APITracer: 5 | def __init__(self, feature, targets): 6 | self.tracer = "ltrace" 7 | self.feature = feature 8 | self.targets = targets 9 | 10 | def config_trace(self): 11 | pass 12 | 13 | def get_timestamp(self, filename="out"): 14 | if not os.path.isfile(filename): 15 | print(f"[tracer] Cannot find trace file {filename}") 16 | return -1 17 | 18 | with open(filename, "r") as fp: 19 | out_raw = fp.readlines() 20 | 21 | for line in out_raw: 22 | if "time1" in line: 23 | t0 = line.strip().split(" ")[-1] 24 | if "." not in t0: 25 | t0 = ( 26 | int(t0) / 1000000.0 27 | ) # c++/rust prints us w/o decimal point 28 | else: 29 | t0 = float(t0) 30 | elif "time2" in line: 31 | t1 = line.strip().split(" ")[-1] 32 | if "." not in t1: 33 | t1 = ( 34 | int(t1) / 1000000.0 35 | ) # c++/rust prints us w/o decimal point 36 | else: 37 | t1 = float(t1) 38 | break 39 | 40 | self.t0 = t0 41 | self.t1 = t1 42 | 43 | def parse_trace(self, filename="trace"): 44 | if not os.path.isfile(filename): 45 | print(f"[tracer] Cannot find trace file {filename}") 46 | return -1 47 | 48 | func_dict = dict() 49 | 50 | with open(filename, "r") as fp: 51 | trace_raw = fp.readlines() 52 | 53 | for line in trace_raw: 54 | line = line.strip() 55 | timestamp = float(line.split(" ")[0]) 56 | 57 | if timestamp < self.t0 or timestamp > self.t1: 58 | continue 59 | 60 | if "" in line: 61 | func_name = line.split(" ")[1].split("@")[0] 62 | retval = None 63 | if func_name in func_dict: 64 | func_dict[func_name].append([timestamp, retval]) 65 | else: 66 | func_dict[func_name] = [[timestamp, retval]] 67 | 68 | elif "<..." in line and "resumed>" in line: 69 | func_name = line.split(" ")[2] 70 | retval = line.split("= ")[-1] 71 | 72 | if func_name not in func_dict: 73 | print(f"[tracer] fatal error: {func_name} not seen yet") 74 | return -1 75 | 76 | # update retval 77 | func_dict[func_name][-1][1] = retval 78 | 79 | elif ".so" in line: 80 | func_name = line.split(" ")[1].split("@")[0] 81 | retval = line.split("= ")[-1] 82 | 83 | if func_name in func_dict: 84 | func_dict[func_name].append([timestamp, retval]) 85 | else: 86 | func_dict[func_name] = [[timestamp, retval]] 87 | 88 | # print(func_dict) 89 | return func_dict 90 | -------------------------------------------------------------------------------- /src/utils/clear_shm.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | ipcrm -m `ipcs -m | grep 0x1d021326 | awk '{print $2}'` 4 | -------------------------------------------------------------------------------- /src/utils/create_msg.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | 3 | from collections import OrderedDict 4 | import pickle 5 | import time 6 | 7 | msg_file = "msgs/msg-1611007897.7347338" # turtle disappears 8 | # msg_file = "msg-1611008929.8656843" # hits the wall 9 | msg_dict = pickle.load(open(msg_file, "rb")) 10 | 11 | print(msg_dict) 12 | 13 | # geometry_msgs/Twist 14 | msg_dict = OrderedDict() 15 | d1 = {"x": 1.933767054481329e+241, "y": 0.0, "z": 0.0} # x 16 | d1 = {"x": 1.933767054481329e+241, "y": 4.8355230813047836e-160, "z": 0.0} # x 17 | d1 = {"x": 1.933767054481329e+241, "y": 4.8355230813047836e-160, "z": -3.958275365874432e+132} 18 | d2 = {"x": -2.6477435485741285e-44, "y": -3.503376746735036e+183, "z": -5.474433982957781e+106} 19 | 20 | d1 = {"x": 0.0, "y": 0.0, "z": 0.0} 21 | d2 = {"x": 0.0, "y": 0.0, "z": -5.474433982957781e+106} 22 | msg_dict["linear"] = OrderedDict(d1) 23 | msg_dict["angular"] = OrderedDict(d2) 24 | 25 | pickle.dump(msg_dict, open("msg-{}".format(time.time()), "wb")) 26 | 27 | """ 28 | OrderedDict([ 29 | ('linear', OrderedDict([ 30 | ('x', 1.933767054481329e+241), 31 | ('y', 4.8355230813047836e-160), 32 | ('z', -3.958275365874432e+132) 33 | ]) 34 | ), 35 | ('angular', OrderedDict([ 36 | ('x', -2.6477435485741285e-44), 37 | ('y', -3.503376746735036e+183), 38 | ('z', -5.474433982957781e+106) 39 | ]) 40 | ) 41 | ]) 42 | """ 43 | 44 | -------------------------------------------------------------------------------- /src/utils/msg_viewer.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import pickle 3 | 4 | msg_file = sys.argv[1].strip() 5 | 6 | with open(msg_file, "r") as f: 7 | msg_dict = pickle.load(f) 8 | 9 | print(msg_dict) 10 | -------------------------------------------------------------------------------- /src/utils/replay.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | 3 | import os 4 | import sys 5 | import pickle 6 | import time 7 | from functools import reduce 8 | import importlib 9 | 10 | from ros_utils import get_msg_class_from_name, get_subscriptions 11 | from fuzzer import flatten_nested_dict 12 | 13 | import rclpy 14 | 15 | 16 | def main(fuzz_type): 17 | rclpy.init() 18 | node = rclpy.create_node("replayer") 19 | time.sleep(1) 20 | 21 | if fuzz_type == "message": 22 | target_node = "/turtlesim" 23 | topic_name = "/turtle1/cmd_vel" 24 | msg_type_class = get_msg_class_from_name("geometry_msgs", "Twist") 25 | msg = msg_type_class() 26 | 27 | if len(sys.argv) > 1: 28 | msg_file = sys.argv[2] 29 | else: 30 | msg_file = "msgs/bug2-2.msg" 31 | 32 | msg_dict = pickle.load(open(msg_file, "rb")) 33 | 34 | l = flatten_nested_dict(msg_dict) 35 | for item in l: 36 | attr_list = item[:-1] 37 | attr_leaf = attr_list[-1] 38 | data_val = item[-1] 39 | 40 | obj = reduce(getattr, attr_list[:-1], msg) 41 | setattr(obj, attr_leaf, data_val) 42 | 43 | # msg.linear.x = 0.0 44 | # msg.linear.y = 0.0 45 | # msg.linear.z = 0.0 46 | # msg.angular.x = 0.0 47 | # msg.angular.y = 0.0 48 | # msg.angular.z = 375561095.7465013 49 | # msg.angular.z = 1.0 50 | print(msg) 51 | 52 | pub = node.create_publisher(msg_type_class, topic_name, 10) 53 | pub.publish(msg) 54 | 55 | time.sleep(1) 56 | 57 | subscriptions = get_subscriptions(node) 58 | assert target_node in subscriptions 59 | 60 | elif fuzz_type == "service": 61 | ldate = "20210219-180042" 62 | ltime = "1613775658.863716" 63 | 64 | meta_file = os.path.join("logs", ldate, "metadata", f"meta-{ltime}") 65 | with open(meta_file, "r") as fp: 66 | meta = fp.readline().split("\t") 67 | srv_name = meta[1] 68 | srv_type_full = meta[2] 69 | 70 | ss = srv_type_full.split("/") 71 | if len(ss) == 2: 72 | ss = [ss[0], "srv", ss[1]] 73 | srv_pkg = ss[0] 74 | srv_type = ss[-1] 75 | 76 | srv_module_name = '.'.join(ss[:-1]) 77 | try: 78 | module = importlib.import_module(srv_module_name) 79 | srv_module = getattr(module, srv_type) 80 | if not srv_pkg or not srv_module: 81 | raise ValueError() 82 | except ValueError: 83 | raise RuntimeError("service type is invalid") 84 | 85 | client = node.create_client(srv_module, srv_name) 86 | request = srv_module.Request() 87 | 88 | req_file = os.path.join("logs", ldate, "queue", f"request-{ltime}") 89 | req_dict = pickle.load(open(req_file, "rb")) 90 | 91 | l = flatten_nested_dict(req_dict) 92 | for item in l: 93 | attr_list = item[:-1] 94 | attr_leaf = attr_list[-1] 95 | data_val = item[-1] 96 | 97 | obj = reduce(getattr, attr_list[:-1], request) 98 | setattr(obj, attr_leaf, data_val) 99 | 100 | print(request) 101 | future = client.call_async(request) 102 | 103 | rclpy.spin_until_future_complete(node, future) 104 | if future.result() is not None: 105 | print('response:\n%r\n' % future.result()) 106 | else: 107 | raise RuntimeError('Exception while calling service: %r' % future.exception()) 108 | 109 | rclpy.shutdown() 110 | 111 | 112 | if __name__ == "__main__": 113 | if len(sys.argv) < 2: 114 | exit(-1) 115 | fuzz_type = sys.argv[1].strip() 116 | if fuzz_type not in ["message", "service", "action"]: 117 | exit(-1) 118 | main(fuzz_type) 119 | -------------------------------------------------------------------------------- /src/utils/reset_focus.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | gsettings reset org.gnome.desktop.wm.preferences focus-new-windows 4 | -------------------------------------------------------------------------------- /src/utils/ros-radamsa.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | i=0 4 | for word in $(aspell -d en dump master | aspell -l en expand | head -n 5); do 5 | echo "{data: \"${word}\"}" > "/tmp/samples/sample-${i}" 6 | i=$((i+1)) 7 | done 8 | 9 | pgrep listener || exit 0 10 | 11 | while true; do 12 | STR=$(/usr/bin/radamsa /tmp/samples/sample-*) 13 | echo "$STR" 14 | (ros2 topic pub --once /chatter \ 15 | std_msgs/String "${STR}" 2>&1) > /dev/null 16 | test $? -gt 127 && break # break on segfaults 17 | pgrep listener || break 18 | done 19 | echo "SEGV" 20 | echo "$STR" > msg 21 | -------------------------------------------------------------------------------- /src/utils/set_focus.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | gsettings set org.gnome.desktop.wm.preferences focus-new-windows 'strict' 4 | -------------------------------------------------------------------------------- /src/utils/speaker.py: -------------------------------------------------------------------------------- 1 | import time 2 | import os 3 | 4 | while True: 5 | t = str(time.time()) 6 | cmd = "ros2 topic pub --once /sros2_input std_msgs/String '{data: " + t + "}'" 7 | os.system(cmd) 8 | -------------------------------------------------------------------------------- /src/utils/sros_init.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | ros2 security create_keystore fuzzing_keys 4 | ros2 security create_key fuzzing_keys /fuzzer/_fuzzer 5 | ros2 security create_key fuzzing_keys /fuzzer/sros2_node 6 | 7 | ros2 security create_permission fuzzing_keys /fuzzer/_fuzzer policies/fuzzer.policy.xml 8 | -------------------------------------------------------------------------------- /src/watchlist/empty.json: -------------------------------------------------------------------------------- 1 | { 2 | } -------------------------------------------------------------------------------- /src/watchlist/idltest.json: -------------------------------------------------------------------------------- 1 | { 2 | "/idltest_Bool_out": "idltest_msgs/msg/Bool", 3 | "/idltest_BoolFixedArray_out": "idltest_msgs/msg/BoolFixedArray", 4 | "/idltest_BoolBoundedDynArray_out": "idltest_msgs/msg/BoolBoundedDynArray", 5 | "/idltest_BoolUnboundedDynArray_out": "idltest_msgs/msg/BoolUnboundedDynArray", 6 | "/idltest_Byte_out": "idltest_msgs/msg/Byte", 7 | "/idltest_ByteFixedArray_out": "idltest_msgs/msg/ByteFixedArray", 8 | "/idltest_ByteBoundedDynArray_out": "idltest_msgs/msg/ByteBoundedDynArray", 9 | "/idltest_ByteUnboundedDynArray_out": "idltest_msgs/msg/ByteUnboundedDynArray", 10 | "/idltest_Char_out": "idltest_msgs/msg/Char", 11 | "/idltest_CharFixedArray_out": "idltest_msgs/msg/CharFixedArray", 12 | "/idltest_CharBoundedDynArray_out": "idltest_msgs/msg/CharBoundedDynArray", 13 | "/idltest_CharUnboundedDynArray_out": "idltest_msgs/msg/CharUnboundedDynArray", 14 | "/idltest_Float32_out": "idltest_msgs/msg/Float32", 15 | "/idltest_Float32FixedArray_out": "idltest_msgs/msg/Float32FixedArray", 16 | "/idltest_Float32BoundedDynArray_out": "idltest_msgs/msg/Float32BoundedDynArray", 17 | "/idltest_Float32UnboundedDynArray_out": "idltest_msgs/msg/Float32UnboundedDynArray", 18 | "/idltest_Float64_out": "idltest_msgs/msg/Float64", 19 | "/idltest_Float64FixedArray_out": "idltest_msgs/msg/Float64FixedArray", 20 | "/idltest_Float64BoundedDynArray_out": "idltest_msgs/msg/Float64BoundedDynArray", 21 | "/idltest_Float64UnboundedDynArray_out": "idltest_msgs/msg/Float64UnboundedDynArray", 22 | "/idltest_Int8_out": "idltest_msgs/msg/Int8", 23 | "/idltest_Int8FixedArray_out": "idltest_msgs/msg/Int8FixedArray", 24 | "/idltest_Int8BoundedDynArray_out": "idltest_msgs/msg/Int8BoundedDynArray", 25 | "/idltest_Int8UnboundedDynArray_out": "idltest_msgs/msg/Int8UnboundedDynArray", 26 | "/idltest_UInt8_out": "idltest_msgs/msg/UInt8", 27 | "/idltest_UInt8FixedArray_out": "idltest_msgs/msg/UInt8FixedArray", 28 | "/idltest_UInt8BoundedDynArray_out": "idltest_msgs/msg/UInt8BoundedDynArray", 29 | "/idltest_UInt8UnboundedDynArray_out": "idltest_msgs/msg/UInt8UnboundedDynArray", 30 | "/idltest_Int16_out": "idltest_msgs/msg/Int16", 31 | "/idltest_Int16FixedArray_out": "idltest_msgs/msg/Int16FixedArray", 32 | "/idltest_Int16BoundedDynArray_out": "idltest_msgs/msg/Int16BoundedDynArray", 33 | "/idltest_Int16UnboundedDynArray_out": "idltest_msgs/msg/Int16UnboundedDynArray", 34 | "/idltest_UInt16_out": "idltest_msgs/msg/UInt16", 35 | "/idltest_UInt16FixedArray_out": "idltest_msgs/msg/UInt16FixedArray", 36 | "/idltest_UInt16BoundedDynArray_out": "idltest_msgs/msg/UInt16BoundedDynArray", 37 | "/idltest_UInt16UnboundedDynArray_out": "idltest_msgs/msg/UInt16UnboundedDynArray", 38 | "/idltest_Int32_out": "idltest_msgs/msg/Int32", 39 | "/idltest_Int32FixedArray_out": "idltest_msgs/msg/Int32FixedArray", 40 | "/idltest_Int32BoundedDynArray_out": "idltest_msgs/msg/Int32BoundedDynArray", 41 | "/idltest_Int32UnboundedDynArray_out": "idltest_msgs/msg/Int32UnboundedDynArray", 42 | "/idltest_UInt32_out": "idltest_msgs/msg/UInt32", 43 | "/idltest_UInt32FixedArray_out": "idltest_msgs/msg/UInt32FixedArray", 44 | "/idltest_UInt32BoundedDynArray_out": "idltest_msgs/msg/UInt32BoundedDynArray", 45 | "/idltest_UInt32UnboundedDynArray_out": "idltest_msgs/msg/UInt32UnboundedDynArray", 46 | "/idltest_Int64_out": "idltest_msgs/msg/Int64", 47 | "/idltest_Int64FixedArray_out": "idltest_msgs/msg/Int64FixedArray", 48 | "/idltest_Int64BoundedDynArray_out": "idltest_msgs/msg/Int64BoundedDynArray", 49 | "/idltest_Int64UnboundedDynArray_out": "idltest_msgs/msg/Int64UnboundedDynArray", 50 | "/idltest_UInt64_out": "idltest_msgs/msg/UInt64", 51 | "/idltest_UInt64FixedArray_out": "idltest_msgs/msg/UInt64FixedArray", 52 | "/idltest_UInt64BoundedDynArray_out": "idltest_msgs/msg/UInt64BoundedDynArray", 53 | "/idltest_UInt64UnboundedDynArray_out": "idltest_msgs/msg/UInt64UnboundedDynArray", 54 | "/idltest_String_out": "idltest_msgs/msg/String", 55 | "/idltest_StringFixedArray_out": "idltest_msgs/msg/StringFixedArray", 56 | "/idltest_StringBoundedDynArray_out": "idltest_msgs/msg/StringBoundedDynArray", 57 | "/idltest_StringUnboundedDynArray_out": "idltest_msgs/msg/StringUnboundedDynArray", 58 | "/idltest_WString_out": "idltest_msgs/msg/WString", 59 | "/idltest_WStringFixedArray_out": "idltest_msgs/msg/WStringFixedArray", 60 | "/idltest_WStringBoundedDynArray_out": "idltest_msgs/msg/WStringBoundedDynArray", 61 | "/idltest_WStringUnboundedDynArray_out": "idltest_msgs/msg/WStringUnboundedDynArray" 62 | } 63 | -------------------------------------------------------------------------------- /src/watchlist/moveit2.json: -------------------------------------------------------------------------------- 1 | { 2 | "/move_action/_action/status": "action_msgs/msg/GoalStatusArray", 3 | "/joint_states": "sensor_msgs/msg/JointState", 4 | "/panda_arm_controller/state": "control_msgs/msg/JointTrajectoryControllerState", 5 | "/motion_plan_request": "moveit_msgs/msg/MotionPlanRequest" 6 | } 7 | -------------------------------------------------------------------------------- /src/watchlist/px4.json: -------------------------------------------------------------------------------- 1 | { 2 | "/ActuatorArmed_PubSubTopic": "px4_msgs/msg/ActuatorArmed", 3 | "/ActuatorOutputs_PubSubTopic": "px4_msgs/msg/ActuatorOutputs", 4 | "/BatteryStatus_PubSubTopic": "px4_msgs/msg/BatteryStatus", 5 | "/ManualControlSetpoint_PubSubTopic": "px4_msgs/msg/ManualControlSetpoint", 6 | "/SensorAccel_PubSubTopic": "px4_msgs/msg/SensorAccel", 7 | "/SensorBaro_PubSubTopic": "px4_msgs/msg/SensorBaro", 8 | "/SensorGps_PubSubTopic": "px4_msgs/msg/SensorGps", 9 | "/SensorGyro_PubSubTopic": "px4_msgs/msg/SensorGyro", 10 | "/SensorsStatusImu_PubSubTopic": "px4_msgs/msg/SensorsStatusImu", 11 | "/SensorCombined_PubSubTopic": "px4_msgs/msg/SensorCombined", 12 | "/Timesync_PubSubTopic": "px4_msgs/msg/Timesync", 13 | "/VehicleAcceleration_PubSubTopic": "px4_msgs/msg/VehicleAcceleration", 14 | "/VehicleAngularAcceleration_PubSubTopic": "px4_msgs/msg/VehicleAngularAcceleration", 15 | "/VehicleAngularVelocity_PubSubTopic": "px4_msgs/msg/VehicleAngularVelocity", 16 | "/VehicleAttitude_PubSubTopic": "px4_msgs/msg/VehicleAttitude", 17 | "/VehicleControlMode_PubSubTopic": "px4_msgs/msg/VehicleControlMode", 18 | "/VehicleLocalPosition_PubSubTopic": "px4_msgs/msg/VehicleLocalPosition", 19 | "/VehicleGlobalPosition_PubSubTopic": "px4_msgs/msg/VehicleLocalPosition", 20 | "/VehicleGpsPosition_PubSubTopic": "px4_msgs/msg/VehicleGpsPosition", 21 | "/VehicleOdometry_PubSubTopic": "px4_msgs/msg/VehicleOdometry", 22 | "/VehicleImu_PubSubTopic": "px4_msgs/msg/VehicleImu", 23 | "/VehicleStatus_PubSubTopic": "px4_msgs/msg/VehicleStatus" 24 | } 25 | -------------------------------------------------------------------------------- /src/watchlist/turtlebot3.json: -------------------------------------------------------------------------------- 1 | { 2 | "/cmd_vel": "geometry_msgs/msg/Twist", 3 | "/imu": "sensor_msgs/msg/Imu", 4 | "/odom": "nav_msgs/msg/Odometry", 5 | "/scan": "sensor_msgs/msg/LaserScan" 6 | } -------------------------------------------------------------------------------- /src/watchlist/turtlesim.json: -------------------------------------------------------------------------------- 1 | { 2 | "turtle1/pose": "turtlesim/msg/Pose" 3 | } --------------------------------------------------------------------------------