ROS2 如何实时判断两个相机的连接状态,用c++编写,要求将两个相机状态以bool的类型存储在一个结构体中,最终返回一个结构体,当无相机话题的时候,要求相机状态为false
时间: 2024-03-25 21:37:01 浏览: 15
可以使用ROS2中的C++ API来实现对两个相机连接状态的实时判断,并将结果存储在结构体中。以下是一个示例代码:
```c++
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/camera_info.hpp>
struct CameraStatus {
bool camera1_connected;
bool camera2_connected;
};
class CameraStatusChecker : public rclcpp::Node {
public:
CameraStatusChecker() : Node("camera_status_checker") {
// Subscribe to camera info topics
camera1_sub_ = create_subscription<sensor_msgs::msg::CameraInfo>(
"/camera1/camera_info", 10, std::bind(&CameraStatusChecker::camera1Callback, this, std::placeholders::_1));
camera2_sub_ = create_subscription<sensor_msgs::msg::CameraInfo>(
"/camera2/camera_info", 10, std::bind(&CameraStatusChecker::camera2Callback, this, std::placeholders::_1));
// Initialize camera status
camera_status_.camera1_connected = false;
camera_status_.camera2_connected = false;
}
// Callback function for camera1
void camera1Callback(const sensor_msgs::msg::CameraInfo::SharedPtr msg) {
camera_status_.camera1_connected = true;
}
// Callback function for camera2
void camera2Callback(const sensor_msgs::msg::CameraInfo::SharedPtr msg) {
camera_status_.camera2_connected = true;
}
CameraStatus getCameraStatus() {
// Reset camera status to false if no message received in the last second
if ((now() - last_msg_time_).seconds() > 1.0) {
camera_status_.camera1_connected = false;
camera_status_.camera2_connected = false;
}
last_msg_time_ = now();
return camera_status_;
}
private:
CameraStatus camera_status_;
rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr camera1_sub_;
rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr camera2_sub_;
rclcpp::Time last_msg_time_;
};
int main(int argc, char **argv) {
rclcpp::init(argc, argv);
auto node = std::make_shared<CameraStatusChecker>();
rclcpp::Rate loop_rate(10); // 10 Hz
while (rclcpp::ok()) {
auto camera_status = node->getCameraStatus();
// Do something with camera status
rclcpp::spin_some(node);
loop_rate.sleep();
}
rclcpp::shutdown();
return 0;
}
```
在上述代码中,我们创建了一个名为`CameraStatusChecker`的节点,该节点订阅了两个相机的`/camera_info`话题,并在回调函数中更新相应相机的连接状态。同时,我们还定义了一个名为`CameraStatus`的结构体,用于存储两个相机的连接状态。在`getCameraStatus`函数中,我们将上述结构体返回,并在函数中判断是否有新消息到达以更新连接状态。最后,在主函数中,我们使用`spin_some`函数来接收消息,并在循环中调用`getCameraStatus`函数来获取相机连接状态。