53 lines
1.3 KiB
C++
53 lines
1.3 KiB
C++
|
//
|
||
|
|
// Created by lgv on 2025/8/27.
|
||
|
|
//
|
||
|
|
|
||
|
|
|
||
|
|
#include "device_manager/device_manager.h"
|
||
|
|
#include "gtest/gtest.h"
|
||
|
|
#include "controller/touch_controller.h"
|
||
|
|
#include "cmvr/msgs/geometry.pb.h"
|
||
|
|
|
||
|
|
|
||
|
|
using namespace cmvr::device;
|
||
|
|
using namespace cmvr::msgs;
|
||
|
|
using namespace cmvr::ctrl;
|
||
|
|
|
||
|
|
TEST(TouchControllerTest,MyTest) {
|
||
|
|
//
|
||
|
|
std::string config_path = "/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml";
|
||
|
|
const XmlNode config(config_path);
|
||
|
|
|
||
|
|
if (!config.hasChild("DeviceManager")){
|
||
|
|
LOG(ERROR) << "Device Manager node not found";
|
||
|
|
}
|
||
|
|
auto dmgr_cfg = config.getChild("DeviceManager");
|
||
|
|
auto &dmgr = DeviceManager::getInstance(dmgr_cfg);
|
||
|
|
|
||
|
|
auto robot = dmgr.getDevice<AbstractRobot>("hc01");
|
||
|
|
auto hand = dmgr.getDevice<AbstractDexHand>("hand1");
|
||
|
|
|
||
|
|
|
||
|
|
cmvr::msgs::Pose3d pose;
|
||
|
|
|
||
|
|
pose.mutable_position()->set_x( 1.49969573e-01);
|
||
|
|
pose.mutable_position()->set_y(-4.00100001e-01);
|
||
|
|
pose.mutable_position()->set_z(-1.00102800e-01);
|
||
|
|
|
||
|
|
pose.mutable_euler()->set_rx(0);
|
||
|
|
pose.mutable_euler()->set_ry(0);
|
||
|
|
pose.mutable_euler()->set_rz(1.57);
|
||
|
|
|
||
|
|
robot->moveJ("PELVIS_S","R_WRIST_R_S",pose);
|
||
|
|
|
||
|
|
Pose3d offset;
|
||
|
|
|
||
|
|
TouchController controller(robot, hand);
|
||
|
|
|
||
|
|
while (true) {
|
||
|
|
controller.touch(pose,offset,500);
|
||
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
}
|