Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Ros2 Diagnostics - Fix for Frame_id naming #640

Merged
merged 6 commits into from
Dec 20, 2024
Merged

Ros2 Diagnostics - Fix for Frame_id naming #640

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
67bd39f
Fixed ros2 diagnostics syntax
Dec 5, 2024
5aed7fa
fixed build error
Dec 5, 2024
fd2c36d
Get frame_id prefix from paramters when using tf publishing is on
Dec 6, 2024
ed3a867
Removed CameraParamHandler from BaseNode
Dec 11, 2024
1697f07
hardware_id for diagnostics to fully_qualified_name to allow it to fi…
Dec 12, 2024
8fd1faa
removed camera_paramter header as it is no longer needed
Dec 13, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,7 @@ extern const std::unordered_map<std::string, dai::ColorCameraProperties::SensorR
extern const std::unordered_map<std::string, dai::CameraControl::FrameSyncMode> fSyncModeMap;
extern const std::unordered_map<std::string, dai::CameraImageOrientation> cameraImageOrientationMap;
bool rsCompabilityMode(std::shared_ptr<rclcpp::Node> node);
std::string tfPrefix(std::shared_ptr<rclcpp::Node> node);
std::string getSocketName(std::shared_ptr<rclcpp::Node> node, dai::CameraBoardSocket socket);
std::string getNodeName(std::shared_ptr<rclcpp::Node> node, NodeNameEnum name);
void basicCameraPub(const std::string& /*name*/,
Expand Down
2 changes: 1 addition & 1 deletion depthai_ros_driver/src/dai_nodes/base_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -46,7 +46,7 @@ std::string BaseNode::getSocketName(dai::CameraBoardSocket socket) {
}

std::string BaseNode::getTFPrefix(const std::string& frameName) {
return std::string(getROSNode()->get_name()) + "_" + frameName;
return sensor_helpers::tfPrefix(getROSNode()) + "_" + frameName;
}

std::string BaseNode::getOpticalTFPrefix(const std::string& frameName) {
Expand Down
7 changes: 7 additions & 0 deletions depthai_ros_driver/src/dai_nodes/sensors/sensor_helpers.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -63,6 +63,13 @@ const std::unordered_map<NodeNameEnum, std::string> NodeNameMap = {
bool rsCompabilityMode(std::shared_ptr<rclcpp::Node> node) {
return node->get_parameter("camera.i_rs_compat").as_bool();
}

std::string tfPrefix(std::shared_ptr<rclcpp::Node> node) {
if(node->get_parameter("camera.i_publish_tf_from_calibration").as_bool()) {
return node->get_parameter("camera.i_tf_base_frame").as_string();
}
return node->get_name();
}
std::string getNodeName(std::shared_ptr<rclcpp::Node> node, NodeNameEnum name) {
if(rsCompabilityMode(node)) {
return rsNodeNameMap.at(name);
Expand Down
19 changes: 17 additions & 2 deletions depthai_ros_driver/src/dai_nodes/sys_logger.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@ void SysLogger::setXinXout(std::shared_ptr<dai::Pipeline> pipeline) {
void SysLogger::setupQueues(std::shared_ptr<dai::Device> device) {
loggerQ = device->getOutputQueue(loggerQName, 8, false);
updater = std::make_shared<diagnostic_updater::Updater>(getROSNode());
updater->setHardwareID(getROSNode()->get_name() + std::string("_") + device->getMxId() + std::string("_") + device->getDeviceName());
updater->setHardwareID(getROSNode()->get_fully_qualified_name() + std::string("_") + device->getMxId() + std::string("_") + device->getDeviceName());
updater->add("sys_logger", std::bind(&SysLogger::produceDiagnostics, this, std::placeholders::_1));
}

Expand Down Expand Up @@ -69,7 +69,22 @@ void SysLogger::produceDiagnostics(diagnostic_updater::DiagnosticStatusWrapper&
auto logData = loggerQ->get<dai::SystemInformation>(std::chrono::seconds(5), timeout);
if(!timeout) {
stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "System Information");
stat.add("System Information", sysInfoToString(*logData));
const dai::SystemInformation& sysInfo = *logData;
stat.add("Leon CSS CPU Usage", sysInfo.leonCssCpuUsage.average * 100);
stat.add("Leon MSS CPU Usage", sysInfo.leonMssCpuUsage.average * 100);
stat.add("Ddr Memory Usage", sysInfo.ddrMemoryUsage.used / (1024.0f * 1024.0f));
stat.add("Ddr Memory Total", sysInfo.ddrMemoryUsage.total / (1024.0f * 1024.0f));
stat.add("Cmx Memory Usage", sysInfo.cmxMemoryUsage.used / (1024.0f * 1024.0f));
stat.add("Cmx Memory Total", sysInfo.cmxMemoryUsage.total);
stat.add("Leon CSS Memory Usage", sysInfo.leonCssMemoryUsage.used / (1024.0f * 1024.0f));
stat.add("Leon CSS Memory Total", sysInfo.leonCssMemoryUsage.total / (1024.0f * 1024.0f));
stat.add("Leon MSS Memory Usage", sysInfo.leonMssMemoryUsage.used / (1024.0f * 1024.0f));
stat.add("Leon MSS Memory Total", sysInfo.leonMssMemoryUsage.total / (1024.0f * 1024.0f));
stat.add("Average Chip Temperature", sysInfo.chipTemperature.average);
stat.add("Leon CSS Chip Temperature", sysInfo.chipTemperature.css);
stat.add("Leon MSS Chip Temperature", sysInfo.chipTemperature.mss);
stat.add("UPA Chip Temperature", sysInfo.chipTemperature.upa);
stat.add("DSS Chip Temperature", sysInfo.chipTemperature.dss);
} else {
stat.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "No Data");
}
Expand Down
Loading