From a775e04a6e48b2817ec007d3d078bd23eefb6b81 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Tue, 23 Jan 2024 12:03:39 +0100 Subject: [PATCH 1/4] Clean up + healtcheck update --- Dockerfile.gazebo | 31 +++--- Dockerfile.hardware | 39 ++++---- demo/compose.build.yaml | 8 +- demo/shm-only.xml | 2 +- healthcheck.cpp | 55 ----------- husarion_utils/healthcheck.cpp | 94 +++++++++++++++++++ .../healthcheck.sh | 0 .../print-serial-number.py | 13 ++- husarion_utils/run_healthcheck_node.sh | 6 ++ .../sync_with_rosbot.sh | 0 run_healthcheck_node.sh | 2 - 11 files changed, 146 insertions(+), 104 deletions(-) delete mode 100644 healthcheck.cpp create mode 100644 husarion_utils/healthcheck.cpp rename healthcheck.sh => husarion_utils/healthcheck.sh (100%) rename print-serial-number.py => husarion_utils/print-serial-number.py (86%) create mode 100644 husarion_utils/run_healthcheck_node.sh rename sync_with_rosbot.sh => husarion_utils/sync_with_rosbot.sh (100%) delete mode 100644 run_healthcheck_node.sh 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 &" From f9a21c64a2d034871c257af0fd21be56bf39a733 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Tue, 23 Jan 2024 15:23:03 +0100 Subject: [PATCH 2/4] fix --- Dockerfile.hardware | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/Dockerfile.hardware b/Dockerfile.hardware index 79f0ced..2cc790c 100644 --- a/Dockerfile.hardware +++ b/Dockerfile.hardware @@ -75,6 +75,13 @@ WORKDIR /ros2_ws COPY --from=ros_builder /ros2_ws /ros2_ws COPY --from=ros_builder /version.txt /version.txt +COPY husarion_utils /husarion_utils + +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 RUN apt-get update && apt-get install -y \ python3-pip \ @@ -95,14 +102,6 @@ RUN apt-get update && apt-get install -y \ rm -rf src && \ rm -rf /var/lib/apt/lists/* -COPY husarion_utils /husarion_utils - -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 - # Run healthcheck in background COPY --from=healthcheck_builder /ros2_ws /healthcheck_ws RUN sed -i "/# /r /husarion_utils/run_healthcheck_node.sh" /*_entrypoint.sh && \ From 8c2631208cd85f884a8dd7c3bbc4f63aeb949b04 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Tue, 23 Jan 2024 19:21:28 +0100 Subject: [PATCH 3/4] microros no namespace --- husarion_utils/healthcheck.cpp | 48 +++++++++++++++++----------------- 1 file changed, 24 insertions(+), 24 deletions(-) diff --git a/husarion_utils/healthcheck.cpp b/husarion_utils/healthcheck.cpp index 0d09668..5fc0c43 100644 --- a/husarion_utils/healthcheck.cpp +++ b/husarion_utils/healthcheck.cpp @@ -2,6 +2,7 @@ #include "nav_msgs/msg/odometry.hpp" #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/imu.hpp" +#include "sensor_msgs/msg/joint_state.hpp" #include using namespace std::chrono; @@ -13,52 +14,51 @@ using namespace std::chrono_literals; 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)); + : Node("healthcheck_rosbot"), last_ekf_msg_(steady_clock::duration(0)), + last_imu_msg_(steady_clock::duration(0)), + last_motors_msg_(steady_clock::duration(0)) { 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(), + "/_imu/data_raw", rclcpp::SystemDefaultsQoS().keep_last(1), std::bind(&HealthCheckNode::imuCallback, this, std::placeholders::_1)); + sub_motors_ = create_subscription( + "/_motors_response", rclcpp::SystemDefaultsQoS().keep_last(1), + std::bind(&HealthCheckNode::motorsCallback, 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_; + steady_clock::time_point last_motors_msg_; - rclcpp::Subscription::SharedPtr sub_controller_; rclcpp::Subscription::SharedPtr sub_ekf_; rclcpp::Subscription::SharedPtr sub_imu_; + rclcpp::Subscription::SharedPtr sub_motors_; 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_; + duration elapsed_time = current_time - last_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_); + bool is_motors = isMsgValid(current_time, last_motors_msg_); - if (is_controller && is_ekf && is_imu) { + if (is_ekf && is_imu && is_motors) { writeHealthStatus("healthy"); } else { writeHealthStatus("unhealthy"); @@ -70,19 +70,19 @@ class HealthCheckNode : public rclcpp::Node { 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(); + RCLCPP_DEBUG_ONCE(get_logger(), "EKF msg arrived"); + last_ekf_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(); + RCLCPP_DEBUG_ONCE(get_logger(), "IMU msg arrived"); + last_imu_msg_ = steady_clock::now(); + } + + void motorsCallback(const sensor_msgs::msg::JointState::SharedPtr msg) { + RCLCPP_DEBUG_ONCE(get_logger(), "Motors msg arrived"); + last_motors_msg_ = steady_clock::now(); } }; From 378ca153735b4a19bc363677da90f5b14209d4a1 Mon Sep 17 00:00:00 2001 From: rafal-gorecki Date: Fri, 26 Jan 2024 09:21:30 +0100 Subject: [PATCH 4/4] Install dependencies step --- Dockerfile.gazebo | 104 ++++++++++++------------- Dockerfile.hardware | 89 +++++++++++---------- husarion_utils/run_healthcheck_node.sh | 0 3 files changed, 98 insertions(+), 95 deletions(-) mode change 100644 => 100755 husarion_utils/run_healthcheck_node.sh diff --git a/Dockerfile.gazebo b/Dockerfile.gazebo index 2b2799e..36b06a2 100644 --- a/Dockerfile.gazebo +++ b/Dockerfile.gazebo @@ -1,9 +1,8 @@ ARG ROS_DISTRO=humble ARG PREFIX= -## =========================== Healthcheck builder =============================== - -FROM husarnet/ros:${PREFIX}${ROS_DISTRO}-ros-base AS healthcheck_builder +## =========================== Install all ROS dependencies =============================== +FROM husarnet/ros:${PREFIX}${ROS_DISTRO}-ros-core AS ros-core-with-dependencies ARG ROS_DISTRO ARG PREFIX @@ -12,9 +11,42 @@ SHELL ["/bin/bash", "-c"] WORKDIR /ros2_ws -RUN mkdir src && \ - cd src && \ - MYDISTRO=${PREFIX:-ros}; MYDISTRO=${MYDISTRO//-/} && \ +RUN apt-get update && apt-get install -y \ + python3-pip \ + python3-rosdep \ + ros-dev-tools && \ + # Clone source + git clone --depth 1 -b humble https://github.com/husarion/rosbot_ros.git src && \ + vcs import src < src/rosbot/rosbot_hardware.repos && \ + vcs import src < src/rosbot/rosbot_simulation.repos && \ + cp -r src/ros2_controllers/diff_drive_controller src/ && \ + cp -r src/ros2_controllers/imu_sensor_broadcaster src/ && \ + rm -rf src/ros2_controllers && \ + # Install dependencies + rm -rf /etc/ros/rosdep/sources.list.d/20-default.list && \ + rosdep init && \ + rosdep update --rosdistro $ROS_DISTRO && \ + rosdep install --from-paths src --ignore-src -y && \ + apt-get remove -y \ + python3-pip \ + python3-rosdep \ + ros-dev-tools && \ + apt-get clean && \ + rm -rf /var/lib/apt/lists/* + + +## =========================== Healthcheck builder =============================== + +FROM ros-core-with-dependencies AS healthcheck_builder + +ARG ROS_DISTRO +ARG PREFIX + +WORKDIR /healthcheck_ws/src + +COPY husarion_utils/healthcheck.cpp /healthcheck.cpp + +RUN 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 sensor_msgs && \ @@ -22,90 +54,56 @@ RUN mkdir src && \ add_executable(healthcheck_node src/healthcheck.cpp)\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 husarion_utils/healthcheck.cpp src/healthcheck_pkg/src/ - -RUN MYDISTRO=${PREFIX:-ros}; MYDISTRO=${MYDISTRO//-/} && \ - source /opt/$MYDISTRO/$ROS_DISTRO/setup.bash && \ + ./healthcheck_pkg/CMakeLists.txt && \ + mv /healthcheck.cpp ./healthcheck_pkg/src/ && \ + cd .. && \ colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release && \ rm -rf build log src ## =========================== ROS builder =============================== -FROM husarnet/ros:${PREFIX}${ROS_DISTRO}-ros-base AS ros_builder +FROM ros-core-with-dependencies AS ros_builder ARG PREFIX -# select bash as default shell -SHELL ["/bin/bash", "-c"] - ENV IGNITION_VERSION fortress ENV HUSARION_ROS_BUILD simulation WORKDIR /ros2_ws # install everything needed -RUN apt-get update --fix-missing && apt-get install -y \ - python3-pip \ - ros-dev-tools && \ - # Clone source - git clone --depth 1 -b humble https://github.com/husarion/rosbot_ros.git src && \ - vcs import src < src/rosbot/rosbot_hardware.repos && \ - vcs import src < src/rosbot/rosbot_simulation.repos && \ - cp -r src/ros2_controllers/diff_drive_controller src/ && \ - cp -r src/ros2_controllers/imu_sensor_broadcaster src/ && \ - rm -rf src/ros2_controllers && \ - # Install dependencies - rm -rf /etc/ros/rosdep/sources.list.d/20-default.list && \ - rosdep init && \ - rosdep update --rosdistro $ROS_DISTRO && \ - rosdep install --from-paths src --ignore-src -y +RUN apt-get update && apt-get install -y \ + ros-dev-tools RUN MYDISTRO=${PREFIX:-ros}; MYDISTRO=${MYDISTRO//-/} && \ source /opt/$MYDISTRO/$ROS_DISTRO/setup.bash && \ - colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release && \ + colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release && \ + echo $(cat /ros2_ws/src/rosbot/package.xml | grep '' | sed -r 's/.*([0-9]+.[0-9]+.[0-9]+)<\/version>/\1/g') >> /version.txt && \ rm -rf build log ## =========================== Final Stage =============================== -FROM husarnet/ros:${PREFIX}${ROS_DISTRO}-ros-core +FROM ros-core-with-dependencies ARG ROS_DISTRO ARG PREFIX -# select bash as default shell -SHELL ["/bin/bash", "-c"] - ENV IGNITION_VERSION fortress ENV HUSARION_ROS_BUILD simulation WORKDIR /ros2_ws COPY --from=ros_builder /ros2_ws /ros2_ws +COPY --from=ros_builder /version.txt /version.txt +COPY husarion_utils /husarion_utils RUN apt-get update && apt-get install -y \ - python3-pip \ - python3-rosdep \ ros-$ROS_DISTRO-teleop-twist-keyboard && \ - rm -rf /etc/ros/rosdep/sources.list.d/20-default.list && \ - rosdep init && \ - rosdep update --rosdistro $ROS_DISTRO && \ - rosdep install -i --from-path src --rosdistro $ROS_DISTRO -y && \ - # Save version - 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-pip \ - python3-rosdep && \ apt-get clean && \ rm -rf src && \ rm -rf /var/lib/apt/lists/* -COPY husarion_utils /husarion_utils - # 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 - +COPY --from=healthcheck_builder /healthcheck_ws /healthcheck_ws +RUN sed -i "/# /i /husarion_utils/run_healthcheck_node.sh" /*_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 2cc790c..beac97a 100644 --- a/Dockerfile.hardware +++ b/Dockerfile.hardware @@ -1,9 +1,8 @@ ARG ROS_DISTRO=humble ARG PREFIX= -## =========================== Healthcheck builder =============================== - -FROM husarnet/ros:${PREFIX}${ROS_DISTRO}-ros-base AS healthcheck_builder +## =========================== Install all ROS dependencies =============================== +FROM husarnet/ros:${PREFIX}${ROS_DISTRO}-ros-core AS ros-core-with-dependencies ARG ROS_DISTRO ARG PREFIX @@ -12,9 +11,41 @@ SHELL ["/bin/bash", "-c"] WORKDIR /ros2_ws -RUN mkdir src && \ - cd src && \ - MYDISTRO=${PREFIX:-ros}; MYDISTRO=${MYDISTRO//-/} && \ +RUN apt-get update && apt-get install -y \ + python3-pip \ + python3-rosdep \ + ros-dev-tools && \ + # Clone source + git clone --depth 1 -b humble https://github.com/husarion/rosbot_ros.git src && \ + vcs import src < src/rosbot/rosbot_hardware.repos && \ + cp -r src/ros2_controllers/diff_drive_controller src/ && \ + cp -r src/ros2_controllers/imu_sensor_broadcaster src/ && \ + rm -rf src/ros2_controllers && \ + rm -r src/rosbot_gazebo && \ + # Install dependencies + rm -rf /etc/ros/rosdep/sources.list.d/20-default.list && \ + rosdep init && \ + rosdep update --rosdistro $ROS_DISTRO && \ + rosdep install --from-paths src --ignore-src -y && \ + apt-get remove -y \ + python3-pip \ + python3-rosdep \ + ros-dev-tools && \ + apt-get clean && \ + rm -rf /var/lib/apt/lists/* + +## =========================== Healthcheck builder =============================== + +FROM ros-core-with-dependencies AS healthcheck_builder + +ARG ROS_DISTRO +ARG PREFIX + +WORKDIR /healthcheck_ws/src + +COPY husarion_utils/healthcheck.cpp /healthcheck.cpp + +RUN 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 sensor_msgs && \ @@ -22,17 +53,14 @@ RUN mkdir src && \ add_executable(healthcheck_node src/healthcheck.cpp)\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 husarion_utils/healthcheck.cpp src/healthcheck_pkg/src/ - -RUN MYDISTRO=${PREFIX:-ros}; MYDISTRO=${MYDISTRO//-/} && \ - source /opt/$MYDISTRO/$ROS_DISTRO/setup.bash && \ + ./healthcheck_pkg/CMakeLists.txt && \ + mv /healthcheck.cpp ./healthcheck_pkg/src/ && \ + cd .. && \ colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release && \ rm -rf build log src ## =========================== ROS builder =============================== -FROM husarnet/ros:${PREFIX}${ROS_DISTRO}-ros-base AS ros_builder +FROM ros-core-with-dependencies AS ros_builder ARG ROS_DISTRO ARG PREFIX @@ -40,37 +68,22 @@ ARG PREFIX SHELL ["/bin/bash", "-c"] WORKDIR /ros2_ws -RUN mkdir src RUN apt-get update && apt-get install -y \ - python3-pip + ros-dev-tools -RUN git clone --depth 1 -b humble https://github.com/husarion/rosbot_ros.git src && \ - vcs import src < src/rosbot/rosbot_hardware.repos && \ - cp -r src/ros2_controllers/diff_drive_controller src/ && \ - cp -r src/ros2_controllers/imu_sensor_broadcaster src/ && \ - rm -rf src/ros2_controllers && \ - # it is necessary to remove simulation - otherwise rosdep tries to install dependencies - rm -r src/rosbot_gazebo && \ - # without this line (using vulcanexus base image) rosdep init throws error: "ERROR: default sources list file already exists:" - rm -rf /etc/ros/rosdep/sources.list.d/20-default.list && \ - rosdep init && \ - rosdep update --rosdistro $ROS_DISTRO && \ - rosdep install --from-paths src --ignore-src -y && \ - MYDISTRO=${PREFIX:-ros}; MYDISTRO=${MYDISTRO//-/} && \ +RUN MYDISTRO=${PREFIX:-ros}; MYDISTRO=${MYDISTRO//-/} && \ source /opt/$MYDISTRO/$ROS_DISTRO/setup.bash && \ colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release && \ echo $(cat /ros2_ws/src/rosbot/package.xml | grep '' | sed -r 's/.*([0-9]+.[0-9]+.[0-9]+)<\/version>/\1/g') >> /version.txt && \ rm -rf build log ## =========================== Final Stage =============================== -FROM husarnet/ros:${PREFIX}${ROS_DISTRO}-ros-core +FROM ros-core-with-dependencies ARG ROS_DISTRO ARG PREFIX -SHELL ["/bin/bash", "-c"] - WORKDIR /ros2_ws COPY --from=ros_builder /ros2_ws /ros2_ws @@ -85,27 +98,19 @@ RUN mv /husarion_utils/print-serial-number.py /usr/bin/ && \ RUN apt-get update && apt-get install -y \ python3-pip \ - python3-rosdep \ stm32flash \ gosu \ ros-$ROS_DISTRO-teleop-twist-keyboard && \ pip3 install pyftdi && \ - rm -rf /etc/ros/rosdep/sources.list.d/20-default.list && \ - rosdep init && \ - rosdep update --rosdistro $ROS_DISTRO && \ - rosdep install -i --from-path src --rosdistro $ROS_DISTRO -y && \ # Size optimalization apt-get remove -y \ - python3-pip \ - python3-rosdep && \ + python3-pip && \ apt-get clean && \ rm -rf src && \ rm -rf /var/lib/apt/lists/* # 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 - +COPY --from=healthcheck_builder /healthcheck_ws /healthcheck_ws +RUN sed -i "/# /i /husarion_utils/run_healthcheck_node.sh" /*_entrypoint.sh HEALTHCHECK --interval=2s --timeout=1s --start-period=30s --retries=1 \ CMD ["/husarion_utils/healthcheck.sh"] diff --git a/husarion_utils/run_healthcheck_node.sh b/husarion_utils/run_healthcheck_node.sh old mode 100644 new mode 100755