diff --git a/Dockerfile.gazebo b/Dockerfile.gazebo index eadc536..2b2799e 100644 --- a/Dockerfile.gazebo +++ b/Dockerfile.gazebo @@ -17,14 +17,14 @@ RUN mkdir src && \ MYDISTRO=${PREFIX:-ros}; MYDISTRO=${MYDISTRO//-/} && \ source /opt/$MYDISTRO/$ROS_DISTRO/setup.bash && \ # Create health check package - ros2 pkg create healthcheck_pkg --build-type ament_cmake --dependencies rclcpp nav_msgs && \ - sed -i '/find_package(nav_msgs REQUIRED)/a \ + ros2 pkg create healthcheck_pkg --build-type ament_cmake --dependencies rclcpp nav_msgs sensor_msgs && \ + sed -i '/find_package(sensor_msgs REQUIRED)/a \ add_executable(healthcheck_node src/healthcheck.cpp)\n \ - ament_target_dependencies(healthcheck_node rclcpp nav_msgs)\n \ + ament_target_dependencies(healthcheck_node rclcpp nav_msgs sensor_msgs)\n \ install(TARGETS healthcheck_node DESTINATION lib/${PROJECT_NAME})' \ /ros2_ws/src/healthcheck_pkg/CMakeLists.txt -COPY healthcheck.cpp src/healthcheck_pkg/src/ +COPY husarion_utils/healthcheck.cpp src/healthcheck_pkg/src/ RUN MYDISTRO=${PREFIX:-ros}; MYDISTRO=${MYDISTRO//-/} && \ source /opt/$MYDISTRO/$ROS_DISTRO/setup.bash && \ @@ -44,13 +44,10 @@ ENV HUSARION_ROS_BUILD simulation WORKDIR /ros2_ws -COPY ./healthcheck.cpp / - # install everything needed RUN apt-get update --fix-missing && apt-get install -y \ python3-pip \ ros-dev-tools && \ - apt-get upgrade -y && \ # Clone source git clone --depth 1 -b humble https://github.com/husarion/rosbot_ros.git src && \ vcs import src < src/rosbot/rosbot_hardware.repos && \ @@ -84,11 +81,10 @@ ENV HUSARION_ROS_BUILD simulation WORKDIR /ros2_ws COPY --from=ros_builder /ros2_ws /ros2_ws -COPY --from=healthcheck_builder /ros2_ws /ros2_ws_healthcheck RUN apt-get update && apt-get install -y \ - python3-rosdep \ python3-pip \ + python3-rosdep \ ros-$ROS_DISTRO-teleop-twist-keyboard && \ rm -rf /etc/ros/rosdep/sources.list.d/20-default.list && \ rosdep init && \ @@ -98,17 +94,18 @@ RUN apt-get update && apt-get install -y \ echo $(cat /ros2_ws/src/rosbot_gazebo/package.xml | grep '' | sed -r 's/.*([0-9]+.[0-9]+.[0-9]+)<\/version>/\1/g') > /version.txt && \ # Size optimalization apt-get remove -y \ - python3-rosdep \ - python3-pip && \ + python3-pip \ + python3-rosdep && \ apt-get clean && \ rm -rf src && \ rm -rf /var/lib/apt/lists/* -COPY healthcheck.sh / -COPY run_healthcheck_node.sh / +COPY husarion_utils /husarion_utils -HEALTHCHECK --interval=2s --timeout=1s --start-period=30s --retries=1 \ - CMD ["/healthcheck.sh"] - -RUN sed -i "/# /r /run_healthcheck_node.sh" /*_entrypoint.sh && \ +# Run healthcheck in background +COPY --from=healthcheck_builder /ros2_ws /healthcheck_ws +RUN sed -i "/# /r /husarion_utils/run_healthcheck_node.sh" /*_entrypoint.sh && \ sed -i "/# /d" /*_entrypoint.sh + +HEALTHCHECK --interval=2s --timeout=1s --start-period=30s --retries=1 \ + CMD ["/husarion_utils/healthcheck.sh"] diff --git a/Dockerfile.hardware b/Dockerfile.hardware index 7f1e1ba..79f0ced 100644 --- a/Dockerfile.hardware +++ b/Dockerfile.hardware @@ -17,14 +17,14 @@ RUN mkdir src && \ MYDISTRO=${PREFIX:-ros}; MYDISTRO=${MYDISTRO//-/} && \ source /opt/$MYDISTRO/$ROS_DISTRO/setup.bash && \ # Create health check package - ros2 pkg create healthcheck_pkg --build-type ament_cmake --dependencies rclcpp nav_msgs && \ - sed -i '/find_package(nav_msgs REQUIRED)/a \ + ros2 pkg create healthcheck_pkg --build-type ament_cmake --dependencies rclcpp nav_msgs sensor_msgs && \ + sed -i '/find_package(sensor_msgs REQUIRED)/a \ add_executable(healthcheck_node src/healthcheck.cpp)\n \ - ament_target_dependencies(healthcheck_node rclcpp nav_msgs)\n \ + ament_target_dependencies(healthcheck_node rclcpp nav_msgs sensor_msgs)\n \ install(TARGETS healthcheck_node DESTINATION lib/${PROJECT_NAME})' \ /ros2_ws/src/healthcheck_pkg/CMakeLists.txt -COPY healthcheck.cpp src/healthcheck_pkg/src/ +COPY husarion_utils/healthcheck.cpp src/healthcheck_pkg/src/ RUN MYDISTRO=${PREFIX:-ros}; MYDISTRO=${MYDISTRO//-/} && \ source /opt/$MYDISTRO/$ROS_DISTRO/setup.bash && \ @@ -42,8 +42,6 @@ SHELL ["/bin/bash", "-c"] WORKDIR /ros2_ws RUN mkdir src -COPY ./healthcheck.cpp / - RUN apt-get update && apt-get install -y \ python3-pip @@ -77,12 +75,6 @@ WORKDIR /ros2_ws COPY --from=ros_builder /ros2_ws /ros2_ws COPY --from=ros_builder /version.txt /version.txt -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 install/rosbot_utils/share/rosbot_utils/firmware/firmware-*.bin /root/firmware.bin RUN apt-get update && apt-get install -y \ python3-pip \ @@ -97,21 +89,24 @@ RUN apt-get update && apt-get install -y \ rosdep install -i --from-path src --rosdistro $ROS_DISTRO -y && \ # Size optimalization apt-get remove -y \ - python3-rosdep \ - python3-pip && \ + python3-pip \ + python3-rosdep && \ apt-get clean && \ rm -rf src && \ rm -rf /var/lib/apt/lists/* -COPY healthcheck.sh / -COPY run_healthcheck_node.sh / +COPY husarion_utils /husarion_utils -COPY print-serial-number.py /usr/bin/ +RUN mv /husarion_utils/print-serial-number.py /usr/bin/ && \ + # for backward compatibility + cp src/rosbot_utils/rosbot_utils/flash-firmware.py /usr/bin/ && \ + cp src/rosbot_utils/rosbot_utils/flash-firmware.py / && \ + cp install/rosbot_utils/share/rosbot_utils/firmware/firmware-*.bin /root/firmware.bin -ENV ROBOT_NAMESPACE= +# Run healthcheck in background +COPY --from=healthcheck_builder /ros2_ws /healthcheck_ws +RUN sed -i "/# /r /husarion_utils/run_healthcheck_node.sh" /*_entrypoint.sh && \ + sed -i "/# /d" /*_entrypoint.sh HEALTHCHECK --interval=2s --timeout=1s --start-period=30s --retries=1 \ - CMD ["/healthcheck.sh"] - -RUN sed -i "/# /r /run_healthcheck_node.sh" /*_entrypoint.sh && \ - sed -i "/# /d" /*_entrypoint.sh + CMD ["/husarion_utils/healthcheck.sh"] diff --git a/demo/compose.build.yaml b/demo/compose.build.yaml index 4731532..486b927 100644 --- a/demo/compose.build.yaml +++ b/demo/compose.build.yaml @@ -1,7 +1,7 @@ # Quick Start -# +# # 1. run `docker compose up` on the robot -# 2. run `ROS_DOMAIN_ID=10 ros2 run teleop_twist_keyboard teleop_twist_keyboard` in the robot termianl +# 2. run `ROS_DOMAIN_ID=10 ros2 run teleop_twist_keyboard teleop_twist_keyboard` in the robot terminal x-common-config: &common-config @@ -27,8 +27,8 @@ services: # command: tail -f /dev/null # command: ros2 run rosbot_utils flash_firmware --usb command: > - ros2 launch rosbot_bringup combined.launch.py + ros2 launch rosbot_bringup combined.launch.py mecanum:=${MECANUM:-False} - serial_port:=$SERIAL_PORT + serial_port:=$SERIAL_PORT serial_baudrate:=576000 # namespace:=rosbot diff --git a/demo/shm-only.xml b/demo/shm-only.xml index 9b9149d..b9f178b 100644 --- a/demo/shm-only.xml +++ b/demo/shm-only.xml @@ -14,4 +14,4 @@ false - \ No newline at end of file + diff --git a/healthcheck.cpp b/healthcheck.cpp deleted file mode 100644 index 0886aee..0000000 --- a/healthcheck.cpp +++ /dev/null @@ -1,55 +0,0 @@ -#include "fstream" -#include "nav_msgs/msg/odometry.hpp" -#include "rclcpp/rclcpp.hpp" -#include - -using namespace std::chrono_literals; - -#define LOOP_PERIOD 500ms -#define MSG_VALID_TIME 2s - -std::chrono::steady_clock::time_point last_msg_time; - -void write_health_status(const std::string &status) { - std::ofstream healthFile("/var/tmp/health_status.txt"); - healthFile << status; -} - -void msg_callback(const nav_msgs::msg::Odometry::SharedPtr msg) { - last_msg_time = std::chrono::steady_clock::now(); -} - -void healthy_check() { - std::chrono::steady_clock::time_point current_time = - std::chrono::steady_clock::now(); - std::chrono::duration elapsed_time = current_time - last_msg_time; - bool is_msg_valid = elapsed_time < MSG_VALID_TIME; - - if (is_msg_valid) { - write_health_status("healthy"); - } else { - write_health_status("unhealthy"); - } -} - -int main(int argc, char *argv[]) { - rclcpp::init(argc, argv); - - std::string topic = "odometry/filtered"; - - if(const char* ns = std::getenv("ROBOT_NAMESPACE")) { - topic = std::string(ns) + "/" + topic; - } - - auto node = rclcpp::Node::make_shared("healthcheck_rosbot"); - auto sub = node->create_subscription( - topic, rclcpp::SensorDataQoS().keep_last(1), msg_callback); - - while (rclcpp::ok()) { - rclcpp::spin_some(node); - healthy_check(); - std::this_thread::sleep_for(LOOP_PERIOD); - } - - return 0; -} diff --git a/husarion_utils/healthcheck.cpp b/husarion_utils/healthcheck.cpp new file mode 100644 index 0000000..0d09668 --- /dev/null +++ b/husarion_utils/healthcheck.cpp @@ -0,0 +1,94 @@ +#include "fstream" +#include "nav_msgs/msg/odometry.hpp" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/imu.hpp" +#include + +using namespace std::chrono; +using namespace std::chrono_literals; + +#define HEALTHCHECK_PERIOD 500ms +#define MSG_VALID_TIME 2s + +class HealthCheckNode : public rclcpp::Node { +public: + HealthCheckNode() + : Node("healthcheck_rosbot"), last_controller_msg_(steady_clock::now()), + last_ekf_msg_(steady_clock::now()), last_imu_msg_(steady_clock::now()) { + + sub_controller_ = create_subscription( + "rosbot_base_controller/odom", + rclcpp::SystemDefaultsQoS().keep_last(1).transient_local(), + std::bind(&HealthCheckNode::controllerCallback, this, + std::placeholders::_1)); + + sub_ekf_ = create_subscription( + "odometry/filtered", rclcpp::SystemDefaultsQoS().keep_last(1), + std::bind(&HealthCheckNode::ekfCallback, this, std::placeholders::_1)); + + sub_imu_ = create_subscription( + "imu_broadcaster/imu", + rclcpp::SystemDefaultsQoS().keep_last(1).transient_local(), + std::bind(&HealthCheckNode::imuCallback, this, std::placeholders::_1)); + + healthcheck_timer_ = create_wall_timer( + HEALTHCHECK_PERIOD, std::bind(&HealthCheckNode::healthyCheck, this)); + } + +private: + steady_clock::time_point last_controller_msg_; + steady_clock::time_point last_ekf_msg_; + steady_clock::time_point last_imu_msg_; + + rclcpp::Subscription::SharedPtr sub_controller_; + rclcpp::Subscription::SharedPtr sub_ekf_; + rclcpp::Subscription::SharedPtr sub_imu_; + rclcpp::TimerBase::SharedPtr healthcheck_timer_; + + bool isMsgValid(steady_clock::time_point current_time, + steady_clock::time_point last_msg) { + duration elapsed_time = current_time - last_controller_msg_; + return elapsed_time < MSG_VALID_TIME; + } + + void healthyCheck() { + auto current_time = steady_clock::now(); + + bool is_controller = isMsgValid(current_time, last_ekf_msg_); + bool is_ekf = isMsgValid(current_time, last_ekf_msg_); + bool is_imu = isMsgValid(current_time, last_imu_msg_); + + if (is_controller && is_ekf && is_imu) { + writeHealthStatus("healthy"); + } else { + writeHealthStatus("unhealthy"); + } + } + + void writeHealthStatus(const std::string &status) { + std::ofstream healthFile("/var/tmp/health_status.txt"); + healthFile << status; + } + + void controllerCallback(const nav_msgs::msg::Odometry::SharedPtr msg) { + RCLCPP_DEBUG_ONCE(get_logger(), "Map msg arrived"); + last_controller_msg_ = steady_clock::now(); + } + + void ekfCallback(const nav_msgs::msg::Odometry::SharedPtr msg) { + RCLCPP_DEBUG_ONCE(get_logger(), "Map msg arrived"); + last_controller_msg_ = steady_clock::now(); + } + + void imuCallback(const sensor_msgs::msg::Imu::SharedPtr msg) { + RCLCPP_DEBUG_ONCE(get_logger(), "Map msg arrived"); + last_controller_msg_ = steady_clock::now(); + } +}; + +int main(int argc, char *argv[]) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/healthcheck.sh b/husarion_utils/healthcheck.sh similarity index 100% rename from healthcheck.sh rename to husarion_utils/healthcheck.sh diff --git a/print-serial-number.py b/husarion_utils/print-serial-number.py similarity index 86% rename from print-serial-number.py rename to husarion_utils/print-serial-number.py index e353cd2..4659d62 100755 --- a/print-serial-number.py +++ b/husarion_utils/print-serial-number.py @@ -6,18 +6,23 @@ import string import re + def get_cpu_id_from_ros_service(): try: # Run the ROS service call command - result = subprocess.run(['ros2', 'service', 'call', '/get_cpu_id', 'std_srvs/srv/Trigger'], - capture_output=True, text=True, timeout=30) + result = subprocess.run( + ["ros2", "service", "call", "/get_cpu_id", "std_srvs/srv/Trigger"], + capture_output=True, + text=True, + timeout=30, + ) # Regular expression to extract JSON from the response match = re.search(r"message='(\{.*\})'", result.stdout) if match: json_str = match.group(1) json_data = json.loads(json_str) - cpu_id = json_data['cpu_id'] + cpu_id = json_data["cpu_id"] return cpu_id else: print("JSON response not found in the output.") @@ -29,6 +34,7 @@ def get_cpu_id_from_ros_service(): print(f"JSON parsing error: {e}") return None + def hex_to_num(hex_str): # Check if the hex string is valid if not all(c in string.hexdigits for c in hex_str): @@ -46,6 +52,7 @@ def hex_to_num(hex_str): # Return the hash as an ASCII string return hash + try: # Obtain CPU ID from ROS service cpu_id = get_cpu_id_from_ros_service() diff --git a/husarion_utils/run_healthcheck_node.sh b/husarion_utils/run_healthcheck_node.sh new file mode 100644 index 0000000..c6c0d86 --- /dev/null +++ b/husarion_utils/run_healthcheck_node.sh @@ -0,0 +1,6 @@ +source "/healthcheck_ws/install/setup.bash" +if [ -n "$ROBOT_NAMESPACE" ]; then + gosu $USER bash -c "ros2 run healthcheck_pkg healthcheck_node --ros-args -r __ns:=/$ROBOT_NAMESPACE &" +else + gosu $USER bash -c "ros2 run healthcheck_pkg healthcheck_node &" +fi diff --git a/sync_with_rosbot.sh b/husarion_utils/sync_with_rosbot.sh similarity index 100% rename from sync_with_rosbot.sh rename to husarion_utils/sync_with_rosbot.sh diff --git a/run_healthcheck_node.sh b/run_healthcheck_node.sh deleted file mode 100644 index 5ed31ae..0000000 --- a/run_healthcheck_node.sh +++ /dev/null @@ -1,2 +0,0 @@ -source "/ros2_ws_healthcheck/install/setup.bash" -gosu $USER bash -c "ros2 run healthcheck_pkg healthcheck_node &"