71 lines
2.1 KiB
C++
71 lines
2.1 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");
|
|
auto cam = dmgr.getDevice<AbstractCamera>("cam4");
|
|
|
|
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);
|
|
// robot->seJ("PELVIS_S","R_WRIST_R_S",pose);
|
|
// robot->servoJ("PELVIS_S","R_WRIST_R_S",pose,0.5);
|
|
|
|
cmvr::msgs::Pose3d delta_pose;
|
|
delta_pose.mutable_position()->set_x( 0.01);
|
|
delta_pose.mutable_position()->set_y(0);
|
|
delta_pose.mutable_position()->set_z(0);
|
|
|
|
delta_pose.mutable_euler()->set_rx(0);
|
|
delta_pose.mutable_euler()->set_ry(0);
|
|
delta_pose.mutable_euler()->set_rz(0);
|
|
|
|
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(10000));
|
|
robot->servoDeltaJ("PELVIS_S","R_FINGER_TIP",delta_pose,0.02);
|
|
|
|
Pose3d offset;
|
|
|
|
TouchController controller(robot, hand,cam);
|
|
|
|
while (true) {
|
|
// controller.touch(pose,offset,500);
|
|
if (controller.isArrive(500)) {
|
|
delta_pose.mutable_position()->set_x( -0.01);
|
|
robot->servoDeltaJ("PELVIS_S","R_FINGER_TIP",delta_pose,0.05);
|
|
}
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
|
}
|
|
|
|
} |