48 lines
1.6 KiB
C++
48 lines
1.6 KiB
C++
#include <chrono>
|
|
#include <iostream>
|
|
#include <memory>
|
|
#include <thread>
|
|
|
|
#include <boost/program_options.hpp>
|
|
|
|
#include <unitree/idl/hg/LowState_.hpp>
|
|
#include <unitree/robot/channel/channel_factory.hpp>
|
|
#include <unitree/robot/channel/channel_subscriber.hpp>
|
|
#include <unitree/robot/h2/common/terminations.hpp>
|
|
|
|
namespace po = boost::program_options;
|
|
using unitree::robot::ChannelFactory;
|
|
using unitree::robot::ChannelSubscriber;
|
|
using unitree_hg::msg::dds_::LowState_;
|
|
|
|
int main(int argc, char** argv) {
|
|
po::options_description options("Unitree H2 termination functions testing.");
|
|
options.add_options()
|
|
("network,n", po::value<std::string>()->default_value(""),
|
|
"dds network interface");
|
|
po::variables_map args;
|
|
po::store(po::parse_command_line(argc, argv, options), args);
|
|
po::notify(args);
|
|
std::cout << options << std::endl;
|
|
|
|
ChannelFactory::Instance()->Init(0, args["network"].as<std::string>());
|
|
|
|
auto lowstate_subscriber =
|
|
std::make_shared<ChannelSubscriber<LowState_>>("rt/lowstate");
|
|
LowState_ lowstate;
|
|
lowstate_subscriber->InitChannel([&lowstate](const void* message) {
|
|
lowstate = *static_cast<const LowState_*>(message);
|
|
});
|
|
|
|
std::cout << "Checking terminations..." << std::endl;
|
|
while (true) {
|
|
if (unitree::robot::h2::bad_orientation(lowstate, 1.0f)) {
|
|
std::cout << "Bad orientation detected!" << std::endl;
|
|
}
|
|
if (unitree::robot::h2::lost_connection(lowstate_subscriber, 1000)) {
|
|
std::cout << "Lost connection!" << std::endl;
|
|
}
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
|
}
|
|
}
|