├── .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 |
--------------------------------------------------------------------------------