From 24fc0c2ca5c5ea007c10b2a5164894c666c929e5 Mon Sep 17 00:00:00 2001 From: Sem van der Hoeven Date: Fri, 3 Mar 2023 14:04:14 +0100 Subject: [PATCH] fix error --- drone_sensors/src/height_sensor.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/drone_sensors/src/height_sensor.cpp b/drone_sensors/src/height_sensor.cpp index e5073731..a700de9a 100644 --- a/drone_sensors/src/height_sensor.cpp +++ b/drone_sensors/src/height_sensor.cpp @@ -13,7 +13,7 @@ public: { publisher_ = this->create_publisher("height_sensor", 10); timer_ = this->create_wall_timer( - 20ms, std::bind(&HeightSensorPublisher::timer_callback, this)); + 500ms, std::bind(&HeightSensorPublisher::timer_callback, this)); RCLCPP_INFO(this->get_logger(), "Starting height sensor publisher"); setup_serial_port(); @@ -34,9 +34,11 @@ private: char *readdata = new char[1]; serial_port.read(readdata, 1); auto message = std_msgs::msg::String(); - message.data = "Height: " + (char)readdata[0]; - if (readdata[0] == 'T') { - RCLCPP_INFO(this->get_logger(), "got start of measurement"); + message.data = "Height: " + std::to_string((char)readdata[0]); + + if (readdata[0] == 'T') + { + RCLCPP_INFO(this->get_logger(), "Height sensor start measurement"); } RCLCPP_INFO(this->get_logger(), "Publishing: %s", message.data.c_str()); publisher_->publish(message); @@ -51,7 +53,7 @@ private: serial_port.open("/dev/ttyACM0", std::ios::in); if (!serial_port.is_open()) { - RCLCPP_ERROR(this->get_logger(), "ERROR Could not open serial port"); + RCLCPP_ERROR(this->get_logger(), "Could not open serial port"); return; } else