76 lines
2.8 KiB
C++
76 lines
2.8 KiB
C++
#include "sensor_msgs/BatteryState.h"
|
|
#include "sensor_msgs/JoyFeedbackArray.h"
|
|
#include "sensor_msgs/PointCloud2.h"
|
|
#include <iostream>
|
|
#include <yaml-cpp/yaml.h>
|
|
|
|
bool getParams()
|
|
{
|
|
try {
|
|
YAML::Node config = YAML::LoadFile("../cfg/config.yaml");
|
|
|
|
double cost_scaling_factor = config["inflation_layer"]["cost_scaling_factor"].as<double>();
|
|
double inflation_radius = config["inflation_layer"]["inflation_radius"].as<double>();
|
|
|
|
bool enabled = config["inflation_layer"]["enabled"].as<bool>();
|
|
bool inflate_unknown = config["inflation_layer"]["inflate_unknown"].as<bool>();
|
|
|
|
double distance = config["obstacle_layer"]["distance"].as<double>();
|
|
|
|
std::cout << "Inflation Layer Parameters:" << std::endl;
|
|
std::cout << " Cost Scaling Factor: " << cost_scaling_factor << std::endl;
|
|
std::cout << " Inflation Radius: " << inflation_radius << std::endl;
|
|
std::cout << " Enabled: " << (enabled ? "true" : "false") << std::endl;
|
|
std::cout << " Inflate Unknown: " << (inflate_unknown ? "true" : "false") << std::endl;
|
|
std::cout << "Obstacle Layer Parameters:" << std::endl;
|
|
std::cout << " Distance: " << distance << std::endl;
|
|
|
|
}
|
|
catch (const YAML::BadFile& e) {
|
|
std::cerr << "Cannot open YAML file: " << e.what() << std::endl;
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
int main()
|
|
{
|
|
getParams();
|
|
sensor_msgs::BatteryState battery;
|
|
|
|
battery.header = std_msgs::Header::now("battery_frame");
|
|
battery.voltage = 12.5f;
|
|
battery.current = -1.2f;
|
|
battery.percentage = 0.85f;
|
|
battery.power_supply_status = sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_DISCHARGING;
|
|
|
|
battery.cell_voltage = {4.15f, 4.18f, 4.12f};
|
|
battery.location = "slot_1";
|
|
battery.serial_number = "SN12345";
|
|
|
|
std::cout << "Battery header timestamp: " << battery.header.stamp << "\n";
|
|
std::cout << "Battery voltage: " << battery.voltage << " V\n";
|
|
std::cout << "Status: " << static_cast<int>(battery.power_supply_status) << "\n";
|
|
std::cout << "Serial Number: " << battery.serial_number << "\n";
|
|
|
|
sensor_msgs::JoyFeedbackArray feedback_array;
|
|
feedback_array.array.push_back(sensor_msgs::JoyFeedback{sensor_msgs::JoyFeedback::TYPE_BUZZER, 0, 5000.0f});
|
|
|
|
std::cout << "First feedback type: " << static_cast<int>(feedback_array.array[0].type) << "\n";
|
|
std::cout << "First feedback intensity: " << feedback_array.array[0].intensity << "\n";
|
|
|
|
sensor_msgs::PointCloud2 cloud;
|
|
cloud.header = std_msgs::Header::now("pointcloud_frame");
|
|
cloud.width = 100;
|
|
cloud.height = 1;
|
|
|
|
std::cout << "PointCloud2 header frame_id: " << cloud.header.frame_id << "\n";
|
|
std::cout << "PointCloud2 header timestamp: " << cloud.header.stamp << "\n";
|
|
std::cout << "PointCloud2 width: " << cloud.width << "\n";
|
|
std::cout << "PointCloud2 height: " << cloud.height << "\n";
|
|
|
|
return 0;
|
|
}
|