├── .gitignore ├── CITATION.cff ├── Dockerfile ├── LICENSE ├── README.md ├── explainable_ros ├── explainable_ros │ ├── __init__.py │ ├── explainability_client_node.py │ ├── explainability_node.py │ ├── vexp_node.py │ └── visual_descriptor_node.py ├── package.xml ├── resource │ └── explainable_ros ├── setup.cfg ├── setup.py └── test │ ├── test_copyright.py │ ├── test_flake8.py │ └── test_pep257.py ├── explainable_ros_bringup ├── CMakeLists.txt ├── launch │ ├── explainable_ros.launch.py │ ├── llama_ros.launch.py │ └── llava_ros.launch.py ├── models │ ├── Qwen2.yaml │ ├── bge-base-en-v1.5.yaml │ └── jina-reranker.yaml └── package.xml ├── explainable_ros_msgs ├── CMakeLists.txt ├── package.xml └── srv │ └── Question.srv ├── images ├── figures │ ├── ArchExplicability.png │ ├── Workflow.png │ └── Workflow_old.png └── logos │ ├── BandaLogos_INCIBE_page-0001.jpg │ ├── logo_demarce.png │ └── logo_edmarce.png ├── requirements.txt └── topic_monitoring ├── package.xml ├── resource └── topic_monitoring ├── setup.cfg ├── setup.py ├── test ├── test_copyright.py ├── test_flake8.py └── test_pep257.py └── topic_monitoring ├── __init__.py └── monitoring_node.py /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode 2 | __pycache__ -------------------------------------------------------------------------------- /CITATION.cff: -------------------------------------------------------------------------------- 1 | cff-version: 1.2.0 2 | message: "If you use this software, please cite it as below." 3 | authors: 4 | - family-names: "Sobrín-Hidalgo" 5 | given-names: "David" 6 | - family-names: "González-Santamarta" 7 | given-names: "Miguel Á." 8 | title: "explainable_ros" 9 | date-released: 2024-24-01 10 | url: "https://github.com/Dsobh/explainable_ros/" 11 | -------------------------------------------------------------------------------- /Dockerfile: -------------------------------------------------------------------------------- 1 | FROM mgons/llama_ros:humble 2 | 3 | #Create workspace and copy repository files 4 | WORKDIR /root/ros2_ws 5 | SHELL ["/bin/bash", "-c"] 6 | COPY . /root/ros2_ws/src 7 | 8 | #Install dependencies 9 | RUN pip3 install -r /src/requirements.txt 10 | 11 | #Colcon and source repository 12 | RUN colcon build 13 | RUN echo "source /install/setup.bash" >> ~/.bashrc 14 | 15 | 16 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2025 David Sobrín Hidalgo 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 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # explainable_ros 2 | 3 | This repository provides a ROS 2 package for generating explanations in autonomous robots (ROS 2) based on log analysis using LLMs. 4 | 5 | Explainable_ros uses the RAG method to filter the most relevant logs from those generated by the robot during the execution of its behavior. 6 | 7 | To enhance the robot's internal data, a VLM is used to process the images captured by the onboard camera, describe them, and log them through rosout. This allows the combination of logs generated by the robot subsystems with images of the environment. The workflow of the system is illustrated in the following figure: 8 | 9 |
System Workflow Image 10 | 11 |
12 | 13 | On the other hand, the high-level representation of the components that make up the developed system is shown in the following figure. 14 | 15 |
System High Level Components 16 | 17 |
18 | 19 | ## Table of Contents 20 | 21 | 1. [How To](#how-to) 22 | - [Installation](#installation) 23 | - [Prerequisites](#prerequisites) 24 | - [explainable_ros Installation Steps](#explainable_ros-installation-steps) 25 | - [Usage](#usage) 26 | - [Local](#local) 27 | - [Docker](#docker) 28 | 2. [Related Works](#related-works) 29 | - [Other Software Projects](#other-software-projects) 30 | - [Related Datasets](#related-datasets) 31 | - [Papers](#papers) 32 | 3. [Cite](#cite) 33 | 4. [Acknowledgments](#acknowledgments) 34 | 35 | ## How To 36 | 37 | ### Installation 38 | 39 | Take into account that the examples shown in the usage section have been made using this [rosbag](https://doi.org/10.5281/zenodo.10896141). 40 | 41 | #### Prerequisites 42 | 43 | You must have llama_ros and CUDA Toolkit (llama_ros dependency) installed. 44 | 45 | #### explainable_ros Installation Steps 46 | 47 | ```shell 48 | cd ros2_ws/src 49 | git clone https://github.com/Dsobh/explainable_ros.git 50 | pip install -r explainable_ros/requirements.txt 51 | 52 | cd ../ 53 | colcon build 54 | ``` 55 | 56 | You can also use docker. To do this you can compile the dockerfile found in the root of this repository or download the corresponding image from Dockerhub. 57 | 58 | ### Usage 59 | 60 | For the examples shown in this section we use the following models (available in llama_ros repository): 61 | 62 | - *Embbeding model*: bge-base-en-v1.5.yaml 63 | - *Reranker model*: jina-reranker 64 | - *Base model*: Qwen2 65 | 66 | #### Local 67 | 68 | - Run the embbeding model: 69 | 70 | ```shell 71 | ros2 llama launch ~/ros2_ws/src/llama_ros/llama_bringup/models/bge-base-en-v1.5.yaml 72 | ``` 73 | 74 | - Run reranker model: 75 | 76 | ```shell 77 | ros2 llama launch ~/ros2_ws/src/llama_ros/llama_bringup/models/jina-reranker.yaml 78 | ``` 79 | 80 | - Run the base model: 81 | 82 | ```shell 83 | ros2 llama launch ~/ros2_ws/src/llama_ros/llama_bringup/models/Qwen2.yaml 84 | ``` 85 | 86 | - Now you can run the main node of the system: 87 | 88 | ```shell 89 | ros2 run explainable_ros explainability_node 90 | ``` 91 | 92 | This node subscribes to the rosout topic and process the logs to add them to the context of the LLM. 93 | You can play a rosbag file in order to generate logs and test the operation of the system. 94 | 95 | 96 | - To request a explanation you should use the /question service: 97 | 98 | ```shell 99 | ros2 service call /question explainable_ros_msgs/srv/Question "{'question': 'What is happening?'}" 100 | ``` 101 | 102 | ##### Example 103 | [ADD IMG] 104 | 105 | #### Docker [WIP] 106 | 107 | Run container: 108 | 109 | ```shell 110 | sudo docker run --rm -it --entrypoint bash 111 | ``` 112 | 113 | Run second container: 114 | 115 | ```shell 116 | docker exec -it bash 117 | ``` 118 | 119 | #### Using VLM component 120 | 121 | - Run a VLM model 122 | 123 | ```shell 124 | ros2 launch llama_bringup minicpm-2.6.launch.py 125 | ``` 126 | 127 | - Run the visual describer node 128 | 129 | ```shell 130 | ros2 run explainable_ros visual_descriptor_node 131 | ``` 132 | 133 | This node is subscribed to the /camera/rgb/image_raw topic and every 5 seconds describes the image captured by the camera and logs it in the /rosout. 134 | 135 | ## Related Works 136 | 137 | ### Other Software Projects 138 | 139 | - [llama_ros](https://github.com/mgonzs13/llama_ros) → A repository that provides a set of ROS 2 packages to integrate llama.cpp into ROS 2. 140 | 141 | ### Related Datasets 142 | 143 | A series of rosbags (ROS 2 Humble) published in Zenodo are listed below. This data can be used to test the explainability capabilities of the project. 144 | 145 | - Sobrín Hidalgo, D. (2024). Navigation Test in Simulated Environment Rosbag. Human obstacle detection. (1.0.0) [Data set]. Robotics Group. https://doi.org/10.5281/zenodo.10896141 146 | - Sobrín Hidalgo, D. (2024). Navigation Benchmark Rosbags Inspired by ERL Competition Test (1.0.0) [Data set]. Robotics Group. https://doi.org/10.5281/zenodo.10518775 147 | 148 | ### Papers 149 | 150 | - Sobrín-Hidalgo, D., González-Santamarta, M. A., Guerrero-Higueras, Á. M., Rodríguez-Lera, F. J., & Matellán-Olivera, V. (2024). [Explaining Autonomy: Enhancing Human-Robot Interaction through Explanation Generation with Large Language Models](https://arxiv.org/abs/2402.04206). arXiv preprint arXiv:2402.04206. 151 | - Sobrín-Hidalgo, D., González-Santamarta, M. Á., Guerrero-Higueras, Á. M., Rodríguez-Lera, F. J., & Matellán-Olivera, V. (2024). [Enhancing Robot Explanation Capabilities through Vision-Language Models: a Preliminary Study by Interpreting Visual Inputs for Improved Human-Robot Interaction](https://arxiv.org/abs/2404.09705). arXiv preprint arXiv:2404.09705. 152 | 153 | ## Cite 154 | 155 | If your work uses this repository, please, cite the repository or the following paper: 156 | 157 | ``` 158 | @article{sobrin2024explaining, 159 | title={Explaining Autonomy: Enhancing Human-Robot Interaction through Explanation Generation with Large Language Models}, 160 | author={Sobr{\'\i}n-Hidalgo, David and Gonz{\'a}lez-Santamarta, Miguel A and Guerrero-Higueras, {\'A}ngel M and Rodr{\'\i}guez-Lera, Francisco J and Matell{\'a}n-Olivera, Vicente}, 161 | journal={arXiv preprint arXiv:2402.04206}, 162 | year={2024} 163 | } 164 | ``` 165 | 166 | ## Acknowledgments 167 | 168 | This project has been partially funded by the Recovery, Transformation, and Resilience Plan, financed by the European Union (Next Generation) thanks to the TESCAC project (Traceability and Explainability in Autonomous Cystems for improved Cybersecurity) granted by INCIBE to the University of León, and by grant PID2021-126592OB-C21 funded by 169 | MCIN/AEI/10.13039/501100011033 EDMAR (Explainable Decision Making in Autonomous Robots) project, PID2021-126592OB-C21 funded by MCIN/AEI/10.13039/501100011033 and by ERDF ”A way of making Europe”. 170 | 171 | 172 | 173 | 174 | 175 | 176 | -------------------------------------------------------------------------------- /explainable_ros/explainable_ros/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Dsobh/explainable_ros/af5aece4bc5494be50952101c101133e73a8edf0/explainable_ros/explainable_ros/__init__.py -------------------------------------------------------------------------------- /explainable_ros/explainable_ros/explainability_client_node.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import rclpy 3 | from rclpy.node import Node 4 | from explainable_ros_msgs.srv import Question 5 | 6 | 7 | class ExplainabilityClientNode(Node): 8 | 9 | def __init__(self) -> None: 10 | super().__init__("explainability_client_node") 11 | 12 | self.client = self.create_client(Question, "question") 13 | 14 | def sed_request(self, question_string: str) -> Question.Response: 15 | 16 | req = Question.Request() 17 | req.question = question_string 18 | 19 | self.client.wait_for_service() 20 | self.future = self.client.call_async(req) 21 | 22 | rclpy.spin_until_future_complete(self, self.future) 23 | return self.future.result() 24 | 25 | 26 | def main(args=None): 27 | rclpy.init(args=args) 28 | 29 | client_node = ExplainabilityClientNode() 30 | response = client_node.sed_request(sys.argv[1]) 31 | print(f"Response: {response.answer}") 32 | client_node.destroy_node() 33 | rclpy.shutdown() 34 | 35 | 36 | if __name__ == "__main__": 37 | main() 38 | -------------------------------------------------------------------------------- /explainable_ros/explainable_ros/explainability_node.py: -------------------------------------------------------------------------------- 1 | import time 2 | 3 | import rclpy 4 | from rclpy.node import Node 5 | from rclpy.executors import MultiThreadedExecutor 6 | from rclpy.callback_groups import ReentrantCallbackGroup 7 | 8 | from rcl_interfaces.msg import Log 9 | from explainable_ros_msgs.srv import Question 10 | 11 | 12 | from langchain_chroma import Chroma 13 | 14 | from langchain_core.output_parsers import StrOutputParser 15 | from langchain_core.runnables import RunnablePassthrough 16 | from langchain_core.messages import SystemMessage 17 | from langchain_core.prompts import ChatPromptTemplate, HumanMessagePromptTemplate 18 | 19 | from langchain.retrievers import ContextualCompressionRetriever 20 | from langchain.schema import Document 21 | 22 | from llama_ros.langchain import ChatLlamaROS, LlamaROSEmbeddings, LlamaROSReranker 23 | 24 | 25 | class ExplainabilityNode(Node): 26 | def __init__(self): 27 | super().__init__("explainability_node") 28 | 29 | self.logs_number = 0 30 | self.total_time = 0 31 | self.embedding_number = 0 32 | self.msg_queue = [] 33 | self.previous_msg = "" 34 | 35 | # To manage a queue of 10 most recent messages and avoid high amount of duplicated messages 36 | self.recent_msgs = [] 37 | self.recent_msgs_conter = 0 38 | 39 | ###ROS Topics and services 40 | 41 | # Create subscription for /rosout topic 42 | self.subscription = self.create_subscription( 43 | Log, 44 | "/rosout", 45 | self.listener_callback, 46 | 1000, 47 | callback_group=ReentrantCallbackGroup(), 48 | ) 49 | 50 | # Create a ROS 2 Service to make question to de model 51 | self.srv = self.create_service( 52 | Question, 53 | "question", 54 | self.question_server_callback, 55 | callback_group=ReentrantCallbackGroup(), 56 | ) 57 | 58 | self.vector_db = Chroma(embedding_function=LlamaROSEmbeddings()) 59 | 60 | self.retriever = self.vector_db.as_retriever(search_kwargs={"k": 20}) 61 | 62 | # Create prompt 63 | prompt = ChatPromptTemplate.from_messages( 64 | [ 65 | SystemMessage( 66 | "You are analyzing ROS 2 logs with image descriptions from a service robot operating strictly indoors, inside an apartment. The robot moves slowly and captures an image every 3 seconds. Your task is to detect potential cyberattacks on the camera, such as image substitution or denial-of-service (DoS). Ignore normal repetition due to slow movement. Instead, flag any sudden, unexplained changes in the environment (e.g., indoor to outdoor), or inconsistent sequences suggesting tampering or falsification.\n\n" 67 | ), 68 | HumanMessagePromptTemplate.from_template( 69 | "Taking into account the following logs:{context}\n\n{question}" 70 | ), 71 | ] 72 | ) 73 | 74 | compressor = LlamaROSReranker(top_n=5) 75 | compression_retriever = ContextualCompressionRetriever( 76 | base_compressor=compressor, base_retriever=self.retriever 77 | ) 78 | 79 | def format_docs(documents): 80 | logs = "" 81 | sortered_list = self.order_retrievals(documents) 82 | 83 | for l in sortered_list: 84 | logs += l 85 | 86 | return logs 87 | 88 | # Create the chain 89 | self.rag_chain = ( 90 | { 91 | "context": compression_retriever | format_docs, 92 | "question": RunnablePassthrough(), 93 | } 94 | | prompt 95 | | ChatLlamaROS(temp=0.0) 96 | | StrOutputParser() 97 | ) 98 | 99 | self.emb_timer = self.create_timer(0.001, self.emb_cb) 100 | 101 | def listener_callback(self, log: Log) -> None: 102 | self.logs_number += 1 103 | 104 | # For not considering llama_ros logs 105 | if log.name != "llama.llama_embedding_node": 106 | self.msg_queue.append(log) 107 | print(f"Log {self.logs_number}: {log.msg}") 108 | 109 | def emb_cb(self) -> None: 110 | 111 | if self.msg_queue: 112 | log = self.msg_queue.pop(0) 113 | start = time.time() 114 | 115 | # Eliminar solo el mensaje anterior 116 | if log.msg != self.previous_msg and log.name != "welcome_app_tiago": 117 | msg_sec = log.stamp.sec 118 | msg_nanosec = log.stamp.nanosec 119 | unix_timestamp = msg_sec + msg_nanosec / 1e9 120 | 121 | self.vector_db.add_texts([str(unix_timestamp) + " - " + log.msg]) 122 | 123 | # self.vector_db.add_texts(texts=[str(unix_timestamp) + " - " + log.msg]) 124 | self.previous_msg = log.msg 125 | self.embedding_number += 1 126 | 127 | emb_time = time.time() - start 128 | self.total_time += emb_time 129 | print( 130 | f"Time to create embedding {self.embedding_number}: {emb_time} | Total time: {self.total_time}" 131 | ) 132 | 133 | def order_retrievals(self, docuemnt_list): 134 | 135 | aux_list = [] 136 | 137 | for d in docuemnt_list: 138 | aux_list.append(d.page_content + "\n") 139 | 140 | return sorted(aux_list) 141 | 142 | def question_server_callback( 143 | self, request: Question.Request, response: Question.Response 144 | ) -> Question.Response: 145 | 146 | question_text = str(request.question) 147 | answer = self.rag_chain.invoke(question_text) 148 | response.answer = answer 149 | return response 150 | 151 | 152 | def main(args=None): 153 | rclpy.init(args=args) 154 | executor = MultiThreadedExecutor() 155 | node = ExplainabilityNode() 156 | executor.add_node(node) 157 | executor.spin() 158 | node.destroy_node() 159 | rclpy.shutdown() 160 | 161 | 162 | if __name__ == "__main__": 163 | main() 164 | -------------------------------------------------------------------------------- /explainable_ros/explainable_ros/vexp_node.py: -------------------------------------------------------------------------------- 1 | import math 2 | 3 | import rclpy 4 | from simple_node import Node 5 | import message_filters 6 | 7 | from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy 8 | from sensor_msgs.msg import Image 9 | from nav_msgs.msg import Path 10 | from llama_msgs.action import GenerateResponse 11 | 12 | 13 | class VisualExplainabilityNode(Node): 14 | def __init__(self): 15 | super().__init__("vexp_node") 16 | 17 | self._action_client = self.create_action_client( 18 | GenerateResponse, "/llava/generate_response") 19 | 20 | self.previous_distance = float('inf') 21 | self.distance_threshold = 5 22 | 23 | qos_profile = QoSProfile( 24 | reliability=ReliabilityPolicy.BEST_EFFORT, 25 | history=HistoryPolicy.KEEP_LAST, 26 | depth=10) 27 | 28 | # subscribers 29 | camera_sub = message_filters.Subscriber( 30 | self, Image, "/camera", qos_profile=10) 31 | plan_sub = message_filters.Subscriber( 32 | self, Path, "/plan", qos_profile=10) 33 | 34 | self._synchronizer = message_filters.ApproximateTimeSynchronizer( 35 | (camera_sub, plan_sub), 10, 0.5) 36 | self._synchronizer.registerCallback(self.obstacle_detection_callback) 37 | 38 | def calculate_distance(self, p1, p2): 39 | distance = math.sqrt((p2.x - p1.x)**2 + 40 | (p2.y - p1.y)**2 + (p2.z - p1.z)**2) 41 | return distance 42 | 43 | def obstacle_detection_callback(self, data: Image, plan: Path): 44 | total_distance = 0 45 | 46 | for i in range(len(plan.poses) - 1): 47 | p1 = plan.poses[i].pose.position 48 | p2 = plan.poses[i + 1].pose.position 49 | distance = self.calculate_distance(p1, p2) 50 | total_distance += distance 51 | 52 | if self.previous_distance != 0 and total_distance > self.previous_distance * self.distance_threshold: 53 | self.get_logger().info("Possible obstacle detected: Distance to the goal increase from {:.2f} meters to {:.2f} meters".format( 54 | self.previous_distance, total_distance)) 55 | 56 | # VLM Code for Image-To-Text 57 | goal = GenerateResponse.Goal() 58 | goal.prompt = "What is in the center of the image?" 59 | goal.image = data 60 | goal.sampling_config.temp = 0.0 61 | goal.reset = True 62 | 63 | self._action_client.wait_for_server() 64 | self._action_client.send_goal(goal) 65 | self._action_client.wait_for_result() 66 | result: GenerateResponse.Result = self._action_client.get_result() 67 | 68 | self.get_logger().info( 69 | f"Camera Log for obstacle detection: {result.response.text}") 70 | 71 | self.previous_distance = total_distance 72 | 73 | 74 | def main(args=None): 75 | rclpy.init(args=args) 76 | node = VisualExplainabilityNode() 77 | node.join_spin() 78 | node.destroy_node() 79 | rclpy.shutdown() 80 | 81 | 82 | if __name__ == "__main__": 83 | main() 84 | -------------------------------------------------------------------------------- /explainable_ros/explainable_ros/visual_descriptor_node.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | import threading 3 | 4 | from simple_node import Node 5 | from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy 6 | from sensor_msgs.msg import Image 7 | from rcl_interfaces.msg import Log 8 | 9 | from llama_ros.llama_client_node import LlamaClientNode 10 | from llama_msgs.action import GenerateResponse 11 | 12 | 13 | class VisualExplainabilityNode(Node): 14 | def __init__(self): 15 | super().__init__("visual_descriptor_node") 16 | 17 | qos_profile = QoSProfile( 18 | reliability=ReliabilityPolicy.RELIABLE, 19 | history=HistoryPolicy.KEEP_LAST, 20 | depth=1, 21 | ) 22 | 23 | self.camera_subscriber = self.create_subscription( 24 | Image, 25 | "/head_front_camera/rgb/image_raw", 26 | self.camera_callback, 27 | qos_profile, 28 | ) 29 | 30 | self.description_publisher = self.create_publisher( 31 | Log, "/camera_descriptions", qos_profile 32 | ) 33 | 34 | self.last_image_msg = None 35 | self.processing = False 36 | self.lock = threading.Lock() 37 | self.message_id = 0 38 | 39 | timer_period = 3.0 # seconds 40 | self.timer = self.create_timer(timer_period, self.timer_callback) 41 | 42 | def camera_callback(self, msg): 43 | self.last_image_msg = msg 44 | 45 | def timer_callback(self): 46 | if self.last_image_msg is not None and not self.processing: 47 | if self.last_image_msg.data: 48 | print("Procesando imagen") 49 | self.processing = True 50 | thread = threading.Thread(target=self.process_image) 51 | thread.start() 52 | 53 | def process_image(self): 54 | with self.lock: 55 | llama_client = LlamaClientNode.get_instance() 56 | 57 | goal = GenerateResponse.Goal() 58 | goal.prompt = ( 59 | "This image was captured by a robot's camera. " 60 | "Describe briefly in this exact format:\n" 61 | "Environment: [Indoor/Outdoor]\n" 62 | "Objects: [List object types present, no counts]\n" 63 | "People: [Present/None]\n" 64 | "Activity: [If obvious, else 'None']\n" 65 | "Do not provide details or explanations. Keep it short." 66 | ) 67 | goal.image = self.last_image_msg 68 | goal.sampling_config.temp = 0.0 69 | goal.reset = True 70 | 71 | vlm_response, vlm_status_code = llama_client.generate_response(goal) 72 | 73 | if vlm_response and vlm_response.response.text: 74 | description = vlm_response.response.text 75 | 76 | self.get_logger().info( 77 | f"Camera description - {vlm_response.response.text}" 78 | ) 79 | 80 | now = self.get_clock().now().to_msg() 81 | 82 | log_msg = Log() 83 | log_msg.stamp = now 84 | log_msg.msg = f"[Camera description {self.message_id}] {description}" 85 | self.description_publisher.publish(log_msg) 86 | self.message_id += 1 87 | else: 88 | self.get_logger().warn("VLM response was empty or failed.") 89 | 90 | self.last_image_msg = None 91 | self.processing = False 92 | 93 | 94 | def main(args=None): 95 | rclpy.init(args=args) 96 | node = VisualExplainabilityNode() 97 | node.join_spin() 98 | node.destroy_node() 99 | rclpy.shutdown() 100 | 101 | 102 | if __name__ == "__main__": 103 | main() 104 | -------------------------------------------------------------------------------- /explainable_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | explainable_ros 5 | 0.5.0 6 | Explainability tool for ROS2 7 | David Sobrín-Hidalgo 8 | MIT 9 | 10 | rosidl_default_generators 11 | 12 | rclpy 13 | rcl_interfaces 14 | sensor_msgs 15 | 16 | rosidl_default_generators 17 | ros2launch 18 | 19 | rosidl_default_generators 20 | 21 | ament_copyright 22 | ament_flake8 23 | ament_pep257 24 | python3-pytest 25 | 26 | 27 | ament_python 28 | 29 | 30 | -------------------------------------------------------------------------------- /explainable_ros/resource/explainable_ros: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Dsobh/explainable_ros/af5aece4bc5494be50952101c101133e73a8edf0/explainable_ros/resource/explainable_ros -------------------------------------------------------------------------------- /explainable_ros/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/explainable_ros 3 | [install] 4 | install_scripts=$base/lib/explainable_ros 5 | -------------------------------------------------------------------------------- /explainable_ros/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import find_packages, setup 2 | 3 | package_name = "explainable_ros" 4 | 5 | setup( 6 | name=package_name, 7 | version="0.5.0", 8 | packages=find_packages(exclude=["test"]), 9 | data_files=[ 10 | ("share/ament_index/resource_index/packages", ["resource/" + package_name]), 11 | ("share/" + package_name, ["package.xml"]), 12 | ], 13 | install_requires=["setuptools"], 14 | zip_safe=True, 15 | maintainer="David Sobrín-Hidalgo", 16 | maintainer_email="dsobh@unileon.es", 17 | description="Explainability tool for ROS2", 18 | license="MIT", 19 | tests_require=["pytest"], 20 | entry_points={ 21 | "console_scripts": [ 22 | "explainability_node = explainable_ros.explainability_node:main", 23 | "explainability_client_node = explainable_ros.explainability_client_node:main", 24 | "vexp_node = explainable_ros.vexp_node:main", 25 | "visual_descriptor_node = explainable_ros.visual_descriptor_node:main", 26 | ], 27 | }, 28 | ) 29 | -------------------------------------------------------------------------------- /explainable_ros/test/test_copyright.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_copyright.main import main 16 | import pytest 17 | 18 | 19 | # Remove the `skip` decorator once the source file(s) have a copyright header 20 | @pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') 21 | @pytest.mark.copyright 22 | @pytest.mark.linter 23 | def test_copyright(): 24 | rc = main(argv=['.', 'test']) 25 | assert rc == 0, 'Found errors' 26 | -------------------------------------------------------------------------------- /explainable_ros/test/test_flake8.py: -------------------------------------------------------------------------------- 1 | # Copyright 2017 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_flake8.main import main_with_errors 16 | import pytest 17 | 18 | 19 | @pytest.mark.flake8 20 | @pytest.mark.linter 21 | def test_flake8(): 22 | rc, errors = main_with_errors(argv=[]) 23 | assert rc == 0, \ 24 | 'Found %d code style errors / warnings:\n' % len(errors) + \ 25 | '\n'.join(errors) 26 | -------------------------------------------------------------------------------- /explainable_ros/test/test_pep257.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_pep257.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.linter 20 | @pytest.mark.pep257 21 | def test_pep257(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found code style errors / warnings' 24 | -------------------------------------------------------------------------------- /explainable_ros_bringup/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(explainable_ros_bringup) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | 11 | install(DIRECTORY 12 | launch 13 | models 14 | DESTINATION share/${PROJECT_NAME}/ 15 | ) 16 | 17 | ament_package() 18 | -------------------------------------------------------------------------------- /explainable_ros_bringup/launch/explainable_ros.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | from launch import LaunchDescription 3 | from launch.actions import GroupAction 4 | from launch_ros.actions import Node, PushRosNamespace 5 | from llama_bringup.utils import create_llama_launch_from_yaml 6 | from ament_index_python.packages import get_package_share_directory 7 | 8 | 9 | def generate_launch_description(): 10 | package_directory = get_package_share_directory("explainable_ros_bringup") 11 | print(f"[Launch] Package directory resolved: {package_directory}") 12 | 13 | embbedings_model = GroupAction( 14 | [ 15 | PushRosNamespace("llama/embeddings"), 16 | create_llama_launch_from_yaml( 17 | os.path.join(package_directory, "models", "bge-base-en-v1.5.yaml") 18 | ), 19 | ] 20 | ) 21 | 22 | reranker_model = GroupAction( 23 | [ 24 | PushRosNamespace("llama/reranker"), 25 | create_llama_launch_from_yaml( 26 | os.path.join(package_directory, "models", "jina-reranker.yaml") 27 | ), 28 | ] 29 | ) 30 | 31 | base_model = GroupAction( 32 | [ 33 | PushRosNamespace("llama/base"), 34 | create_llama_launch_from_yaml( 35 | os.path.join(package_directory, "models", "Qwen2.yaml") 36 | ), 37 | ] 38 | ) 39 | 40 | explainability_node_cmd = Node( 41 | package="explainable_ros", 42 | executable="explainability_node", 43 | name="explainability_node_main", 44 | output="screen", 45 | ) 46 | 47 | ld = LaunchDescription() 48 | ld.add_action(embbedings_model) 49 | ld.add_action(reranker_model) 50 | ld.add_action(base_model) 51 | ld.add_action(explainability_node_cmd) 52 | 53 | return ld 54 | -------------------------------------------------------------------------------- /explainable_ros_bringup/launch/llama_ros.launch.py: -------------------------------------------------------------------------------- 1 | 2 | from launch import LaunchDescription 3 | from llama_bringup.utils import create_llama_launch 4 | 5 | 6 | def generate_launch_description(): 7 | 8 | return LaunchDescription([ 9 | create_llama_launch( 10 | n_ctx=2048, 11 | n_batch=256, 12 | n_gpu_layers=33, 13 | n_threads=1, 14 | n_predict=-1, 15 | 16 | # model_repo="TheBloke/dolphin-2.1-mistral-7B-GGUF", 17 | # model_filename="dolphin-2.1-mistral-7b.Q4_K_M.gguf", 18 | # model_repo="TheBloke/OpenHermes-2.5-neural-chat-7B-v3-1-7B-GGUF", 19 | # model_filename="openhermes-2.5-neural-chat-7b-v3-1-7b.Q4_K_M.gguf", 20 | # model_repo="koesn/multi_verse_model-7B-GGUF", 21 | # model_filename="multi_verse_model.Q4_K_M.gguf", 22 | model_repo="liminerity/M7-7b-GGUF", 23 | model_filename="multiverse-experiment-slerp-7b.Q5_K_M.gguf", 24 | 25 | stopping_words=["INST"], 26 | 27 | debug=False 28 | ) 29 | ]) 30 | -------------------------------------------------------------------------------- /explainable_ros_bringup/launch/llava_ros.launch.py: -------------------------------------------------------------------------------- 1 | # MIT License 2 | 3 | # Copyright (c) 2024 Miguel Ángel González Santamarta 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 | 23 | 24 | from launch import LaunchDescription 25 | from llama_bringup.utils import create_llama_launch 26 | 27 | 28 | def generate_launch_description(): 29 | 30 | return LaunchDescription([ 31 | create_llama_launch( 32 | use_llava=True, 33 | embedding=False, 34 | 35 | n_ctx=8192, 36 | n_batch=512, 37 | n_gpu_layers=33, 38 | n_threads=1, 39 | n_predict=8192, 40 | 41 | model_repo="guinmoon/MobileVLM-3B-GGUF", 42 | model_filename="MobileVLM-3B-Q4_K_M.gguf", 43 | 44 | mmproj_repo="guinmoon/MobileVLM-3B-GGUF", 45 | mmproj_filename="MobileVLM-3B-mmproj-f16.gguf", 46 | 47 | prefix="[INST]", 48 | suffix="[/INST]", 49 | stopping_words=["[INST]"], 50 | 51 | system_prompt_type="mistral", 52 | 53 | debug=False 54 | ) 55 | ]) 56 | -------------------------------------------------------------------------------- /explainable_ros_bringup/models/Qwen2.yaml: -------------------------------------------------------------------------------- 1 | n_ctx: 2048 2 | n_batch: 8 3 | n_gpu_layers: 0 4 | n_threads: 1 5 | n_predict: 2048 6 | 7 | model_repo: "Qwen/Qwen2.5-Coder-7B-Instruct-GGUF" 8 | model_filename: "qwen2.5-coder-7b-instruct-q4_k_m-00001-of-00002.gguf" 9 | 10 | system_prompt_type: "ChatML" -------------------------------------------------------------------------------- /explainable_ros_bringup/models/bge-base-en-v1.5.yaml: -------------------------------------------------------------------------------- 1 | n_ctx: 2048 2 | n_batch: 1024 3 | n_gpu_layers: 0 4 | n_threads: 1 5 | n_predict: 2048 6 | embedding: true 7 | 8 | model_repo: "CompendiumLabs/bge-base-en-v1.5-gguf" 9 | model_filename: "bge-base-en-v1.5-f16.gguf" -------------------------------------------------------------------------------- /explainable_ros_bringup/models/jina-reranker.yaml: -------------------------------------------------------------------------------- 1 | n_ctx: 2048 2 | n_batch: 1024 3 | n_gpu_layers: 0 4 | n_threads: 1 5 | n_predict: 2048 6 | reranking: true 7 | 8 | model_repo: "gpustack/jina-reranker-v1-tiny-en-GGUF" 9 | model_filename: "jina-reranker-v1-tiny-en-FP16.gguf" -------------------------------------------------------------------------------- /explainable_ros_bringup/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | explainable_ros_bringup 5 | 0.5.0 6 | explainable_ros_bringup 7 | David Sobrín-Hidalgo 8 | MIT 9 | 10 | ament_cmake 11 | 12 | ament_lint_auto 13 | ament_lint_common 14 | 15 | explainable_ros 16 | 17 | 18 | ament_cmake 19 | 20 | 21 | -------------------------------------------------------------------------------- /explainable_ros_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(explainable_ros_msgs) 3 | 4 | # find dependencies 5 | find_package(ament_cmake REQUIRED) 6 | find_package(rosidl_default_generators REQUIRED) 7 | 8 | rosidl_generate_interfaces(${PROJECT_NAME} 9 | "srv/Question.srv" 10 | DEPENDENCIES 11 | ) 12 | 13 | ament_package() 14 | -------------------------------------------------------------------------------- /explainable_ros_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | explainable_ros_msgs 5 | 0.0.0 6 | explainable_ros_msgs 7 | David Sobrín-Hidalgo 8 | MIT 9 | 10 | ament_cmake 11 | 12 | ament_lint_auto 13 | ament_lint_common 14 | 15 | rosidl_interface_packages 16 | 17 | 18 | 19 | ament_cmake 20 | rosidl_default_generators 21 | rosidl_default_runtime 22 | 23 | 24 | -------------------------------------------------------------------------------- /explainable_ros_msgs/srv/Question.srv: -------------------------------------------------------------------------------- 1 | string question 2 | --- 3 | string answer -------------------------------------------------------------------------------- /images/figures/ArchExplicability.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Dsobh/explainable_ros/af5aece4bc5494be50952101c101133e73a8edf0/images/figures/ArchExplicability.png -------------------------------------------------------------------------------- /images/figures/Workflow.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Dsobh/explainable_ros/af5aece4bc5494be50952101c101133e73a8edf0/images/figures/Workflow.png -------------------------------------------------------------------------------- /images/figures/Workflow_old.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Dsobh/explainable_ros/af5aece4bc5494be50952101c101133e73a8edf0/images/figures/Workflow_old.png -------------------------------------------------------------------------------- /images/logos/BandaLogos_INCIBE_page-0001.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Dsobh/explainable_ros/af5aece4bc5494be50952101c101133e73a8edf0/images/logos/BandaLogos_INCIBE_page-0001.jpg -------------------------------------------------------------------------------- /images/logos/logo_demarce.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Dsobh/explainable_ros/af5aece4bc5494be50952101c101133e73a8edf0/images/logos/logo_demarce.png -------------------------------------------------------------------------------- /images/logos/logo_edmarce.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Dsobh/explainable_ros/af5aece4bc5494be50952101c101133e73a8edf0/images/logos/logo_edmarce.png -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | sentence_transformers==4.0.1 2 | chromadb==0.6.3 -------------------------------------------------------------------------------- /topic_monitoring/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | topic_monitoring 5 | 0.0.0 6 | TODO: Package description 7 | dsobh 8 | TODO: License declaration 9 | 10 | 11 | rclpy 12 | rcl_interfaces 13 | tf2_msgs 14 | geometry_msgs 15 | 16 | ament_copyright 17 | ament_flake8 18 | ament_pep257 19 | python3-pytest 20 | 21 | 22 | ament_python 23 | 24 | 25 | -------------------------------------------------------------------------------- /topic_monitoring/resource/topic_monitoring: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Dsobh/explainable_ros/af5aece4bc5494be50952101c101133e73a8edf0/topic_monitoring/resource/topic_monitoring -------------------------------------------------------------------------------- /topic_monitoring/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/topic_monitoring 3 | [install] 4 | install_scripts=$base/lib/topic_monitoring 5 | -------------------------------------------------------------------------------- /topic_monitoring/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import find_packages, setup 2 | 3 | package_name = 'topic_monitoring' 4 | 5 | setup( 6 | name=package_name, 7 | version='0.0.0', 8 | packages=find_packages(exclude=['test']), 9 | data_files=[ 10 | ('share/ament_index/resource_index/packages', 11 | ['resource/' + package_name]), 12 | ('share/' + package_name, ['package.xml']), 13 | ], 14 | install_requires=['setuptools'], 15 | zip_safe=True, 16 | maintainer='dsobh', 17 | maintainer_email='davidsobrin@gmail.com', 18 | description='TODO: Package description', 19 | license='TODO: License declaration', 20 | tests_require=['pytest'], 21 | entry_points={ 22 | 'console_scripts': [ 23 | 'monitoring_node = topic_monitoring.monitoring_node:main' 24 | ], 25 | }, 26 | ) 27 | -------------------------------------------------------------------------------- /topic_monitoring/test/test_copyright.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_copyright.main import main 16 | import pytest 17 | 18 | 19 | # Remove the `skip` decorator once the source file(s) have a copyright header 20 | @pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') 21 | @pytest.mark.copyright 22 | @pytest.mark.linter 23 | def test_copyright(): 24 | rc = main(argv=['.', 'test']) 25 | assert rc == 0, 'Found errors' 26 | -------------------------------------------------------------------------------- /topic_monitoring/test/test_flake8.py: -------------------------------------------------------------------------------- 1 | # Copyright 2017 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_flake8.main import main_with_errors 16 | import pytest 17 | 18 | 19 | @pytest.mark.flake8 20 | @pytest.mark.linter 21 | def test_flake8(): 22 | rc, errors = main_with_errors(argv=[]) 23 | assert rc == 0, \ 24 | 'Found %d code style errors / warnings:\n' % len(errors) + \ 25 | '\n'.join(errors) 26 | -------------------------------------------------------------------------------- /topic_monitoring/test/test_pep257.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_pep257.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.linter 20 | @pytest.mark.pep257 21 | def test_pep257(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found code style errors / warnings' 24 | -------------------------------------------------------------------------------- /topic_monitoring/topic_monitoring/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Dsobh/explainable_ros/af5aece4bc5494be50952101c101133e73a8edf0/topic_monitoring/topic_monitoring/__init__.py -------------------------------------------------------------------------------- /topic_monitoring/topic_monitoring/monitoring_node.py: -------------------------------------------------------------------------------- 1 | import rclpy 2 | from rclpy.node import Node 3 | from geometry_msgs.msg import Twist 4 | from rcl_interfaces.msg import Log 5 | from datetime import datetime, timedelta 6 | 7 | 8 | class MonitoringNode(Node): 9 | 10 | def __init__(self): 11 | super().__init__('monitoring_node') 12 | 13 | self.log_pub = self.create_publisher(Log, '/rosout', 10) 14 | self.subscription = self.create_subscription( 15 | Twist, 16 | 'topic', 17 | self.listener_callback, 18 | 10) 19 | 20 | # self.get_logger().info("Starting to monitor cmd_vel topic. Expected min publication rate: 10Hz. Expected max publication rate: 20Hz.") 21 | 22 | self.last_printed_second = None 23 | self.msg_count = 0 24 | self.last_msg_time = None 25 | self.total_msg_count = 0 26 | self.total_time_elapsed = timedelta(seconds=0) 27 | self.timer = self.create_timer(1.0, self.timer_callback) 28 | self.previous_rate = 0.0 29 | self.avg_frequency = 0.0 30 | self.rate_threshold_percentage = 50 31 | 32 | def log_builder(self, avg_rate, string, sec, nanosec): 33 | log_msg = Log() 34 | log_msg.name = self.get_name() 35 | log_msg.msg = f"cmd_vel publication rate {string} from {self.previous_rate:.2f} to {avg_rate:.2f} Hz" 36 | log_msg.stamp.sec = sec 37 | log_msg.stamp.nanosec = nanosec 38 | print(log_msg.msg) 39 | self.log_pub.publish(log_msg) 40 | 41 | def listener_callback(self, msg): 42 | current_time = datetime.now() 43 | if self.last_msg_time is not None: 44 | time_diff = (current_time - self.last_msg_time) 45 | self.total_time_elapsed += time_diff 46 | self.msg_count += 1 47 | self.last_msg_time = current_time 48 | 49 | def timer_callback(self): 50 | stamp_time = self.get_clock().now() 51 | stamp_sec = stamp_time.nanoseconds // 10**9 52 | stamp_nanosec = stamp_time.nanoseconds % 10**9 53 | current_time = datetime.now() 54 | 55 | if self.msg_count > 0: 56 | time_diff = (current_time - self.last_msg_time) 57 | self.total_time_elapsed += time_diff 58 | self.total_msg_count += self.msg_count 59 | 60 | self.avg_frequency = self.total_msg_count / \ 61 | self.total_time_elapsed.total_seconds() 62 | 63 | # threshold_value_up = self.previous_rate * \ 64 | # (1 + self.rate_threshold_percentage / 100) 65 | # threshold_value_down = self.previous_rate * \ 66 | # (1 - self.rate_threshold_percentage / 100) 67 | 68 | # if self.avg_frequency > threshold_value_up and self.previous_rate > 1: 69 | # self.log_builder(self.avg_frequency, "increase", 70 | # stamp_sec, stamp_nanosec) 71 | # if self.avg_frequency < threshold_value_down and self.previous_rate > 1: 72 | # self.log_builder(self.avg_frequency, "decrease", 73 | # stamp_sec, stamp_nanosec) 74 | 75 | self.msg_count = 0 76 | self.last_msg_time = current_time 77 | self.previous_rate = self.avg_frequency 78 | 79 | # Print average rate each second 80 | if current_time.second % 5 == 0 and current_time.second != self.last_printed_second and self.avg_frequency != 0.0: 81 | log_msg = Log() 82 | log_msg.name = self.get_name() 83 | log_msg.msg = f"Average publication rate of cmd_vel: {self.avg_frequency:.2f}Hz." 84 | log_msg.stamp.sec = stamp_sec 85 | log_msg.stamp.nanosec = stamp_nanosec 86 | print(log_msg.msg) 87 | self.log_pub.publish(log_msg) 88 | self.last_printed_second = current_time.second 89 | 90 | 91 | def main(args=None): 92 | rclpy.init(args=args) 93 | 94 | monitoring_node = MonitoringNode() 95 | rclpy.spin(monitoring_node) 96 | monitoring_node.destroy_node() 97 | rclpy.shutdown() 98 | 99 | 100 | if __name__ == '__main__': 101 | main() 102 | --------------------------------------------------------------------------------