{"id":11670,"date":"2025-10-27T17:39:43","date_gmt":"2025-10-27T16:39:43","guid":{"rendered":"https:\/\/dronesonen.usn.no\/?p=11670"},"modified":"2025-12-08T17:11:52","modified_gmt":"2025-12-08T16:11:52","slug":"astrorover-week-10-tony-stark-at-home","status":"publish","type":"post","link":"https:\/\/dronesonen.usn.no\/?p=11670","title":{"rendered":"ASTROROVER – WEEK 10 – TONY STARK at Home"},"content":{"rendered":"\n
ign topic -i \/model\/vehicle_blue\/odometry<\/code><\/pre>\n\n\n\n
August:<\/h2>\n\n\n\n
Implementing IMU for Autonomous Car<\/h3>\n\n\n\n
Finally, everything is ready! This week will be like shark week on Discovery Channel, just instead of sharks, there will be autonomous cars. This will be frickin awesome. I have done a lot of research. I will use this github repo: https:\/\/github.com\/adityakamath\/sensehat_ros<\/a>. This have everything we will need. We will use the IMU later for mapping. Sander is going strong on making the LiDAR work, and we will collaborate later on to make the car fully autonomous. The Ultrasonic Sensor is more or less ready. For the autonomous driving we envision, we need IMU data and LiDAR. The car will after future iterations do mapping and A*.<\/p>\n\n\n\n
IMU – Iteration<\/h3>\n\n\n\n
The goal of this iterations is to get IMU data visualized in RVIZ2 the RPI4 Sense HAT moves. With the github repo I mentioned above, we will get the ROS2 node and topic to be able to do this. We will see if it is plug and play. Time will tell. <\/p>\n\n\n\n
\"\"<\/figure>\n\n\n\n
\"\"<\/figure>\n\n\n\n
I started of course by cloning the repo in \/ros2_ws\/src<\/p>\n\n\n\n
git clone https:\/\/github.com\/adityakamath\/sensehat_ros.git<\/code><\/pre>\n\n\n\n
I then built the workspace and sourced the files. The code uses clpy.qos.qos_profile_sensor_data to be able to deal with high frequent sensor voltage.<\/p>\n\n\n\n
I then launched the node:<\/p>\n\n\n\n
ros2 launch sensehat_ros sensehat_launch.py<\/code><\/pre>\n\n\n\n
I checked the ros2 topic list, and the topic was up. I veryfied that the node was publishing, by doing ros2 topic echo \/IMU. Everything worked well. I then wanted to try to see the visualization in rviz2. I Opened rviz2, and added \/IMU, but nothing happend. The terminal said:
<\/p>\n\n\n\n
\"\"<\/figure>\n\n\n\n
This happend because the default reliability policy was set to reliable, but the code demanded best effort. After fixing this, I was challenged by the next task. I was in the wrong fixed frame, and because of this, I recieved this message in the terminal:<\/p>\n\n\n\n
\"\"<\/figure>\n\n\n\n
After realizing this i tried to move around the RPi4. Nothing happend. When i pitched, roled og yawned the RPi4, nothing happend. This was fixed by:<\/p>\n\n\n\n
ros2 run tf2_ros static_transform_publisher 0 0 0 0 0 0 world sensehat_frame\n<\/code><\/pre>\n\n\n\n
Without this, there will be no movement when trying to visualize IMU in rviz2. This took some time to research, but find this: https:\/\/docs.vulcanexus.org\/en\/jazzy\/ros2_documentation\/source\/Tutorials\/Intermediate\/Tf2\/Writing-A-Tf2-Static-Broadcaster-Py.html<\/a>.<\/p>\n\n\n\n
When I did this, I had movement! Wohooo! This was a milestone, because getting here took quite some time. The problem here, however, was that the z-axis was turned upside down. I realized that it was becausethe tf2 was set 0 0 0 0 0. I had to type 0 0 0 0 3.14159 0. After doing this the axis was the right way. The best one was ros2 run tf2_ros static_transform_publisher 0 0 0 3.14159 3.14159 0 world sensehat_frame<\/p>\n\n\n\n
\"\"<\/figure>\n\n\n\n
Now my display looked like this:<\/p>\n\n\n\n
\"\"<\/figure>\n\n\n\n
Now, the next problem was that the y-axis yawed, and the z-axis rolled. I had to go in to code, and change the order of which they where implemented. By changing yaw and roll, it was corrected.<\/p>\n\n\n\n
The next issue was that the y-axis roled the opposite way of how i roled the RPi4. I had to go into the code, and remove the – in front of the y. After doing this everything worked.<\/p>\n\n\n\n
Making the Gui Great Again – MOST INSANE GUI ITERATION SO FAR<\/h4>\n\n\n\n
Its time to get everything tied together in the GUI. At least the data from the Arduino, the IR ZeroCam and the \/driving_mode topic. I wanted to really optimize everything. I have done a lot of research, searched for hours. The results? Satisfying!<\/p>\n\n\n\n
I removed a lot of the placeholders from last week. I want it more clean, and that the data displayed is useful. I also want the GUI more functional, and that it is easy to see if the nodes are connected or disconnected. This is the most clean the gui has ever been. I removed all unnecessary complexity. I gave it my all, and this is how the design looks like (for now):<\/p>\n\n\n\n
\"\"<\/figure>\n\n\n\n
\"\"<\/figure>\n\n\n\n
I started her: https:\/\/docs.ros.org\/en\/foxy\/Tutorials\/Beginner-Client-Libraries\/Writing-A-Simple-Py-Publisher-And-Subscriber.html?utm_source=chatgpt.com<\/a>. And: https:\/\/docs.ros2.org\/foxy\/api\/std_msgs\/msg\/String.html?utm_source=chatgpt.com<\/a>. And of course: https:\/\/docs.ros.org\/en\/humble\/Tutorials\/Beginner-Client-Libraries\/Writing-A-Simple-Py-Publisher-And-Subscriber.html?utm_source=chatgpt.com<\/a>. If there is a god, this is him: https:\/\/github.com\/adityakamath\/sensehat_ros\/blob\/humble\/README.md<\/a>. https:\/\/docs.ros.org\/en\/foxy\/Concepts\/About-Executors.html<\/a><\/p>\n\n\n\n
Displaying in GUI<\/h4>\n\n\n\n
The first thing I wanted to start with was the pressure data provided by the Sense HAT. The reason why is because I just had validated that the \/pressure topic existed, and that the Sense_HAT_node published to it. This is the code I started to implement her, and I reused it later https:\/\/docs.ros.org\/en\/humble\/Tutorials\/Beginner-Client-Libraries\/Writing-A-Simple-Py-Publisher-And-Subscriber.html?utm_source=chatgpt.com#write-the-subscriber-node<\/a>, https:\/\/gist.github.com\/robosam2003\/9e8cb1d8581ddd3af098a8813c64e71e, <\/a>https:\/\/github.com\/tasada038\/pyqt_ros2_app\/blob\/master\/README.md.<\/a> (I have used more sites as well, they are all listed in the code):<\/p>\n\n\n\n
pressure_signal = pyqtSignal(float)\n\nself.pressure_signal.connect(self.update_pressure_label)\n\ndef update_pressure_label(self, value):\n        self.lblPressuseSenseHAT.setText(f\"Pressure: {value:.1f} hPa\")\n\nself.pressure_subscription = self.create_subscription(\n            FluidPressure,\n            '\/pressure',\n            self.pressure_callback,\n            qos_profile=qos_profile_sensor_data\n        )\n\ndef pressure_callback(self, msg: FluidPressure):\n  pressure_hpa = round(msg.fluid_pressure \/ 100.0,1)\n  self.get_logger().info(f\"Pressure:{pressure_hpa} hpa\")\n  self.interface.update_pressure_label(pressure_hpa)\n  self.interface.pressure_signal.emit(pressure_hpa)\n<\/code><\/pre>\n\n\n\n
The whole idea here is to have a GUI node that can manage and display data and components within in the sytem. I wanted to display the pressure data in the lblPressureSenseHAT, but I met big challenges:<\/p>\n\n\n\n
\"\"<\/figure>\n\n\n\n
Thankfully this was similar to the challenge from getting the Sense HAT visualisation in rviz2. I had to switch policy. https:\/\/docs.ros.org\/en\/humble\/Concepts\/Intermediate\/About-Quality-of-Service-Settings.html<\/a>. In the code aboq the qos_profil=qod_profile_sensor_data is the policy that was needed, I replaced 10 for that. Where 10 was the reliabilty policy, we now have the best effort policy.<\/p>\n\n\n\n
I have done some research on QT graphical view: https:\/\/www.pythonguis.com\/tutorials\/pyqt-qgraphics-vector-graphics\/?utm_source=chatgpt.com<\/a>.<\/p>\n\n\n\n
Now I reused this code, and fixed the same for temperature, humidity and made a publisher.<\/p>\n\n\n\n
Making the Camera a ROS2 Node<\/h3>\n\n\n\n
 I did some research here, and discovered V4L2, https:\/\/docs.ros.org\/en\/humble\/p\/v4l2_camera\/<\/a>. I had to install and initalize the ROS” driver for V4L2:<\/p>\n\n\n\n
sudo apt update\nsudo apt install ros-humble-v4l2-camera\n<\/code><\/pre>\n\n\n\n
Starting the node<\/p>\n\n\n\n
ros2 run v4l2_camera_node<\/code><\/pre>\n\n\n\n
This publishes sensor_msgs\/Image on topic \/image_raw. I then tested if I had a video feed:<\/p>\n\n\n\n
ros2 run image_view image_view --ros-args image:=\/image_raw<\/code><\/pre>\n\n\n\n
This opened a window with a live-feed from the camera. At this moment i knew thtat the ROS2-topic worked, and I was a bit happy and relieved.<\/p>\n\n\n\n
\"\"<\/figure>\n\n\n\n
\"\"<\/figure>\n\n\n\n
I had to use rclpy for ROS”-communication<\/p>\n\n\n\n
cv_bridge + OpenCV in order to convert from sensor_msgs\/Image til QImage.<\/p>\n\n\n\n
And the same PyQt5 signal for secure threading of updates in the GUI.<\/p>\n\n\n\n
Screenshot: https:\/\/stackoverflow.com\/questions\/10381854\/how-to-create-screenshot-of-qwidget<\/a> bilder med pixmap<\/p>\n\n\n\n
Displaying Readings of RPi4 Processor Temperature<\/h4>\n\n\n\n
I did some research and found this: https:\/\/github.com\/Infinite-Echo\/ros2_temperature_tracker.git<\/a>. I then:<\/p>\n\n\n\n
cd \/ros2_ws\/src\ngit clone https:\/\/github.com\/Infinite-Echo\/ros2_temperature_tracker.git\ncd ..\ncolcon build --symlink-install\nsource install\/setup.bash<\/code><\/pre>\n\n\n\n
I then struggled a bit to get it working. The problem, however, was that my RPi4 used a different zone name. On my RPi4 it is cpu-thermal. After changing this, the node published on \/cpu_temp fine! I then did the same process as listed inn pressure, and it got displayed in the GUI.<\/p>\n\n\n\n
Driving_mode topic<\/h4>\n\n\n\n
This topic makes it possible to control and monitor the driving mode form the GUI in real-time via ROS2. https:\/\/docs.ros.org\/en\/humble\/Tutorials\/Beginner-Client-Libraries\/Writing-A-Simple-Py-Publisher-And-Subscriber.html?utm_source=chatgpt.com<\/a>, https:\/\/gist.github.com\/robosam2003\/9e8cb1d8581ddd3af098a8813c64e71e<\/a>, https:\/\/www.pythonguis.com\/docs\/qpushbutton\/?utm_source=chatgpt.com<\/a>. The subscriptions was the same as the procedure in pressure. The buttons and the publishing node:<\/p>\n\n\n\n
self.driving_mode_publisher = self.create_publisher(\n            String,\n            '\/driving_mode',\n            10\n        )\n      self.btnControlDrivingMode.clicked.connect(self.set_joystick_mode)\n        self.btnAutonomousDrivingMode.clicked.connect(self.set_autonomous_mode)<\/code><\/pre>\n\n\n\n
Demonstration of GUI:<\/h4>\n\n\n\n