sem/5g_drone_ROS2
1
0
Fork 0
Code Issues 1 Pull Requests 1 Actions Packages Projects Releases Wiki Activity

add check start of measurement test

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-03-03 13:59:11 +01:00
parent a6fd08738c
commit 31503b8b70
1 changed files with 7 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
drone_sensors/src/height_sensor.cpp
View File

@@ -13,8 +13,8 @@ public:
{ {
publisher_ = this->create_publisher<std_msgs::msg::String>("height_sensor", 10); publisher_ = this->create_publisher<std_msgs::msg::String>("height_sensor", 10);
timer_ = this->create_wall_timer( timer_ = this->create_wall_timer(
500ms, std::bind(&HeightSensorPublisher::timer_callback, this)); 20ms, std::bind(&HeightSensorPublisher::timer_callback, this));
RCLCPP_INFO(this->get_logger(), "Constructor of height sensor publisher"); RCLCPP_INFO(this->get_logger(), "Starting height sensor publisher");
setup_serial_port(); setup_serial_port();
} }
@@ -34,7 +34,10 @@ private:
char *readdata = new char[1]; char *readdata = new char[1];
serial_port.read(readdata, 1); serial_port.read(readdata, 1);
auto message = std_msgs::msg::String(); auto message = std_msgs::msg::String();
message.data = "Height: " + std::to_string((int)readdata[0]); message.data = "Height: " + (char)readdata[0];
if (readdata[0] == 'T') {
RCLCPP_INFO(this->get_logger(), "got start of measurement");
}
RCLCPP_INFO(this->get_logger(), "Publishing: %s", message.data.c_str()); RCLCPP_INFO(this->get_logger(), "Publishing: %s", message.data.c_str());
publisher_->publish(message); publisher_->publish(message);
} }
@@ -48,7 +51,7 @@ private:
serial_port.open("/dev/ttyACM0", std::ios::in); serial_port.open("/dev/ttyACM0", std::ios::in);
if (!serial_port.is_open()) if (!serial_port.is_open())
{ {
RCLCPP_ERROR(this->get_logger(), "Could not open serial port"); RCLCPP_ERROR(this->get_logger(), "ERROR Could not open serial port");
return; return;
} }
else else
@@ -73,11 +76,6 @@ int main(int argc, char *argv[])
executor.add_node(node); executor.add_node(node);
executor.spin(); executor.spin();
// rclcpp::spin(std::make_shared<HeightSensorPublisher>()); // rclcpp::spin(std::make_shared<HeightSensorPublisher>());
while (true)
{
node.get()->test();
}
rclcpp::shutdown(); rclcpp::shutdown();
return 0; return 0;