Skip to content

Commit

Permalink
fix healthcheck
Browse files Browse the repository at this point in the history
  • Loading branch information
DominikN committed Dec 11, 2023
1 parent b964868 commit b3b4a3e
Show file tree
Hide file tree
Showing 4 changed files with 37 additions and 4 deletions.
7 changes: 5 additions & 2 deletions Dockerfile.hardware
Original file line number Diff line number Diff line change
Expand Up @@ -99,7 +99,8 @@ COPY --from=healthcheck_builder /ros2_ws /ros2_ws_healthcheck

# for backward compatibility
RUN cp src/rosbot_utils/rosbot_utils/flash-firmware.py /usr/bin/ && \
cp src/rosbot_utils/rosbot_utils/flash-firmware.py /
cp src/rosbot_utils/rosbot_utils/flash-firmware.py / && \
cp install/rosbot_utils/share/rosbot_utils/firmware/firmware-*.bin /root/firmware.bin

RUN apt-get update && apt-get install -y \
python3-pip \
Expand Down Expand Up @@ -129,5 +130,7 @@ COPY healthcheck.sh /

COPY print-serial-number.py /usr/bin/

HEALTHCHECK --interval=5s --timeout=2s --start-period=5s --retries=4 \
ENV ROS_NAMESPACE=

HEALTHCHECK --interval=5s --timeout=2s --start-period=5s --retries=6 \
CMD ["/healthcheck.sh"]
20 changes: 20 additions & 0 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,26 @@ Docker Image for ROS Humble Node providing interface for STM32 firmware over [Mi

With *docker-compose* configuration shown in [demo](./demo) it can communicate with hardware of ROSbot 2R, ROSbot 2 PRO and ROSbot 2.0.

## Quick Start

```yaml
services:

rosbot:
image: husarion/rosbot:humble
devices:
- ${SERIAL_PORT:?err}
- /dev/bus/usb/ # FTDI (if connecting over USB port with STM32)
environment:
- ROS_DOMAIN_ID=30
command: >
ros2 launch rosbot_bringup combined.launch.py
mecanum:=${MECANUM:-False}
serial_port:=$SERIAL_PORT
serial_baudrate:=576000
namespace:=robot1
```
## Flashing the firmware
Firmware if flashed from inside of the container running on the ROSbot:
Expand Down
5 changes: 4 additions & 1 deletion demo/namespace/compose.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -15,13 +15,16 @@ services:
build:
context: ../..
dockerfile: Dockerfile.hardware
# image: husarion/rosbot:humble-ros2-robot-utils
network_mode: host
ipc: host
devices:
- ${SERIAL_PORT:?err}
- /dev/bus/usb/
- /dev/bus/usb/ # FTDI
environment:
- FASTRTPS_DEFAULT_PROFILES_FILE=/shm-only.xml
- ROS_DOMAIN_ID=30
- ROS_NAMESPACE=robot1 # For healthckeck node
command: >
ros2 launch rosbot_bringup combined.launch.py
mecanum:=${MECANUM:-False}
Expand Down
9 changes: 8 additions & 1 deletion healthcheck.cpp
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
#include "fstream"
#include "nav_msgs/msg/odometry.hpp"
#include "rclcpp/rclcpp.hpp"
#include <cstdlib>

using namespace std::chrono_literals;

Expand Down Expand Up @@ -33,9 +34,15 @@ void healthy_check() {

int main(int argc, char *argv[]) {
rclcpp::init(argc, argv);

std::string topic = "odometry/filtered";
if(const char* ns = std::getenv("ROS_NAMESPACE")) {
topic = std::string(ns) + "/" + topic;
}

auto node = rclcpp::Node::make_shared("healthcheck_node");
auto sub = node->create_subscription<nav_msgs::msg::Odometry>(
"odometry/filtered", rclcpp::SensorDataQoS().keep_last(1), msg_callback);
topic, rclcpp::SensorDataQoS().keep_last(1), msg_callback);

while (rclcpp::ok()) {
rclcpp::spin_some(node);
Expand Down

0 comments on commit b3b4a3e

Please sign in to comment.