SmartBench/cmvr-es
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: fix google test

Browse Source
This commit is contained in:
lgv 2026-02-27 16:31:37 +08:00
parent 021b14b387
commit 539e07e5c7
7 changed files with 121 additions and 82 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
assets/toppra Submodule

@ -0,0 +1 @@
Subproject commit 3089c7897a5711aceb39d25919aca8c57b5c5948

74
cmvr-es/common/config/cabin_robot.xml
View File

@ -32,44 +32,44 @@
<!-- <LeftArm id="left_arm" devtype="ti5Robot" />--> <!-- <LeftArm id="left_arm" devtype="ti5Robot" />-->
<!-- <RightArm />--> <!-- <RightArm />-->
<!-- <Neck/>--> <!-- <Neck/>-->
<!-- <Humanoid id="hc01" dof="14"--> <Humanoid id="hc01" dof="14"
<!-- urdf="/home/lgv/cmvr/cmvr-es/config/robot_description/hc_description/dual_arm.urdf"--> urdf="/home/lgv/cmvr/cmvr-es/model/xiaoyan_description/dual_arm.urdf"
<!-- baseLink="PELVIS_S"--> baseLink="PELVIS_S"
<!-- jointNames="L_SHOULDER_P,L_SHOULDER_R,L_SHOULDER_Y,L_ELBOW_R,L_WRIST_P,L_WRIST_Y,L_WRIST_R,R_SHOULDER_P,R_SHOULDER_R,R_SHOULDER_Y,R_ELBOW_R,R_WRIST_P,R_WRIST_Y,R_WRIST_R"--> jointNames="L_SHOULDER_P,L_SHOULDER_R,L_SHOULDER_Y,L_ELBOW_R,L_WRIST_P,L_WRIST_Y,L_WRIST_R,R_SHOULDER_P,R_SHOULDER_R,R_SHOULDER_Y,R_ELBOW_R,R_WRIST_P,R_WRIST_Y,R_WRIST_R"
<!-- linkNames="PELVIS_S,L_SHOULDER_P_S,L_SHOULDER_R_S,L_SHOULDER_Y_S,L_ELBOW_R_S,L_WRIST_P_S,L_WRIST_Y_S,L_WRIST_R_S,R_SHOULDER_P_S,R_SHOULDER_R_S,R_SHOULDER_Y_S,R_ELBOW_R_S,R_WRIST_P_S,R_WRIST_Y_S,R_WRIST_R_S,R_FINGER_TIP,R_CAM"--> linkNames="PELVIS_S,L_SHOULDER_P_S,L_SHOULDER_R_S,L_SHOULDER_Y_S,L_ELBOW_R_S,L_WRIST_P_S,L_WRIST_Y_S,L_WRIST_R_S,R_SHOULDER_P_S,R_SHOULDER_R_S,R_SHOULDER_Y_S,R_ELBOW_R_S,R_WRIST_P_S,R_WRIST_Y_S,R_WRIST_R_S,R_FINGER_TIP,R_CAM"
<!-- bufferSize="50"--> bufferSize="50"
<!-- verbose="false">--> verbose="false">
<!-- <CanManger id="" devId="">--> <CanManger id="" devId="">
<!-- <LeftArmCan id = " " devId = " " channelId ="0" enable="false" toolFrame="L_FINGER_TIP">--> <LeftArmCan id = " " devId = " " channelId ="0" enable="false" toolFrame="L_FINGER_TIP">
<!-- <Motor id="23" jointName="L_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>--> <Motor id="23" jointName="L_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<!-- <Motor id="24" jointName="L_SHOULDER_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>--> <Motor id="24" jointName="L_SHOULDER_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<!-- <Motor id="25" jointName="L_SHOULDER_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>--> <Motor id="25" jointName="L_SHOULDER_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<!-- <Motor id="26" jointName="L_ELBOW_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>--> <Motor id="26" jointName="L_ELBOW_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<!-- <Motor id="27" jointName="L_WRIST_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>--> <Motor id="27" jointName="L_WRIST_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<!-- <Motor id="21" jointName="L_WRIST_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>--> <Motor id="28" jointName="L_WRIST_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<!-- <Motor id="22" jointName="L_WRIST_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>--> <Motor id="29" jointName="L_WRIST_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<!-- </LeftArmCan>--> </LeftArmCan>
<!-- <RightArmCan id = " " devId = " " channelId ="1" enable="true" toolFrame="R_FINGER_TIP">--> <RightArmCan id = " " devId = " " channelId ="1" enable="true" toolFrame="R_FINGER_TIP">
<!-- <Motor id="16" jointName="R_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>--> <Motor id="16" jointName="R_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<!-- <Motor id="17" jointName="R_SHOULDER_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>--> <Motor id="17" jointName="R_SHOULDER_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<!-- <Motor id="18" jointName="R_SHOULDER_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>--> <Motor id="18" jointName="R_SHOULDER_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<!-- <Motor id="19" jointName="R_ELBOW_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>--> <Motor id="19" jointName="R_ELBOW_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<!-- <Motor id="20" jointName="R_WRIST_P" limitQLb="-3.14" limitQUb="3.14" limitQd="3.0"/>--> <Motor id="20" jointName="R_WRIST_P" limitQLb="-3.14" limitQUb="3.14" limitQd="3.0"/>
<!-- <Motor id="28" jointName="R_WRIST_Y" limitQLb="-1.102" limitQUb="1.02" limitQd="3.0"/>--> <Motor id="21" jointName="R_WRIST_Y" limitQLb="-1.102" limitQUb="1.02" limitQd="3.0"/>
<!-- <Motor id="29" jointName="R_WRIST_R" limitQLb="-0.293" limitQUb="1.57079" limitQd="3.0"/>--> <Motor id="22" jointName="R_WRIST_R" limitQLb="-0.293" limitQUb="1.57079" limitQd="3.0"/>
<!-- </RightArmCan>--> </RightArmCan>
<!-- <HeadCan id = " " devId = " " channelId ="2" enable="true">--> <HeadCan id = " " devId = " " channelId ="2" enable="true">
<!-- <Motor id="32" jointName="HEAD_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>--> <Motor id="32" jointName="HEAD_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<!-- <Motor id="30" jointName="HEAD_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>--> <Motor id="30" jointName="HEAD_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<!-- <Motor id="31" jointName="HEAD_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>--> <Motor id="31" jointName="HEAD_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<!-- </HeadCan>--> </HeadCan>
<!-- <WaistCan id = " " devId = " " channelId ="3" enable="false">--> <WaistCan id = " " devId = " " channelId ="3" enable="false">
<!-- <Motor id="4" jointName="WAIST_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>--> <Motor id="4" jointName="WAIST_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<!-- <Motor id="15" jointName="WAIST_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>--> <Motor id="15" jointName="WAIST_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<!-- </WaistCan>--> </WaistCan>
<!-- </CanManger>--> </CanManger>
<!-- </Humanoid>--> </Humanoid>
</Robot> </Robot>
<BioHead> <BioHead>

6
cmvr-es/common/config/camera_config/camera_config.pb.txt
View File

@ -1,6 +1,6 @@
realsense_cameras { realsense_cameras {
id: "cam2" id: "cam2"
serialNumber: "123456789012" serialNumber: "243122072252"
width: 1280 width: 1280
height: 720 height: 720
fps: 30 fps: 30
@ -10,7 +10,7 @@ realsense_cameras {
align_mode: ALIGN_MODE_COLOR align_mode: ALIGN_MODE_COLOR
buffer_size: 30 buffer_size: 30
sync: true sync: true
enable: false enable: true
} }
uvc_cameras { uvc_cameras {
@ -23,5 +23,5 @@ uvc_cameras {
camera_mode: CAMERA_MODE_PHOTO camera_mode: CAMERA_MODE_PHOTO
stream_mode: STREAM_MODE_RGB stream_mode: STREAM_MODE_RGB
buffer_size: 30 buffer_size: 30
enable: true enable: false
} }

14
cmvr-es/common/utils/config_helper/src/config_setting.cpp
View File

@ -2,13 +2,17 @@
#include <unistd.h> #include <unistd.h>
// static std::string basePath()
// {
// char* cwd = ::getcwd(nullptr, 0);
// if (!cwd) return {};
// std::string s(cwd);
// std::free(cwd);
// return s + "/config/";
// }
static std::string basePath() static std::string basePath()
{ {
char* cwd = ::getcwd(nullptr, 0); return "/home/lgv/cmvr/cmvr-es/output/bin/config/";
if (!cwd) return {};
std::string s(cwd);
std::free(cwd);
return s + "/config/";
} }
DEFINE_string(pinocchio_qp_ik_solver_config_file, DEFINE_string(pinocchio_qp_ik_solver_config_file,

21
cmvr-es/controller/CMakeLists.txt
View File

@ -1,5 +1,5 @@
find_package(VISP REQUIRED) #find_package(VISP REQUIRED)
# 如果报 relocation ... can not be used when making a shared object; recompile with -fPIC ，说明SRC 中包含了test文件 ，test # 如果报 relocation ... can not be used when making a shared object; recompile with -fPIC ，说明SRC 中包含了test文件 ，test
@ -32,7 +32,24 @@ target_link_libraries(controller PUBLIC
ccd ccd
fcl fcl
cmvr_es::device_manager cmvr_es::device_manager
${VISP_LIBRARIES} visp_vs
visp_visual_features
visp_vision
visp_tt_mi
visp_tt
visp_me
visp_mbt
visp_klt
visp_dnn_tracker
visp_blob
visp_sensor
visp_robot
visp_io
visp_imgproc
visp_gui
visp_detection
visp_core
visp_ar
pinocchio_default pinocchio_default
pinocchio_parsers pinocchio_parsers
) )

38
cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp
View File

@ -108,7 +108,7 @@ TEST(HumanoidRobotTest,MyRobotTest) {
} }
TEST(HumanoidRobotTest,ServoJAndGetJointQSmokeTest) { TEST(HumanoidRobotTest,ServoJAndGetJointQSmokeTest) {
const XmlNode config("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml"); const XmlNode config("/home/lgv/cmvr/cmvr-es/cmvr-es/common/config/cabin_robot.xml");
ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found"; ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found";
auto dmgr_cfg = config.getChild("DeviceManager"); auto dmgr_cfg = config.getChild("DeviceManager");
auto& dmgr = DeviceManager::getInstance(dmgr_cfg); auto& dmgr = DeviceManager::getInstance(dmgr_cfg);
@ -130,18 +130,18 @@ TEST(HumanoidRobotTest,ServoJAndGetJointQSmokeTest) {
ASSERT_TRUE(refreshRightArmQ()) << "Failed to read right-arm joint q"; ASSERT_TRUE(refreshRightArmQ()) << "Failed to read right-arm joint q";
// std::vector<JointPoint> cmd{}; std::vector<JointPoint> cmd{};
// cmd = { cmd = {
// {"R_SHOULDER_P", 0.0}, // {"R_SHOULDER_P", 0.0},
// {"R_SHOULDER_R", 0.0}, // {"R_SHOULDER_R", 0.0},
// {"R_SHOULDER_Y", 0.0}, // {"R_SHOULDER_Y", 0.0},
// {"R_ELBOW_R", 0.0}, // {"R_ELBOW_R", 0.0},
// {"R_WRIST_P", 0.0}, // {"R_WRIST_P", 0.0},
// {"R_WRIST_Y", 0.0}, // {"R_WRIST_Y", 0.0},
// {"R_WRIST_R", 0.0}, {"R_WRIST_R", 0.2},
// }; };
auto joint_cmd = buildRightArmJointCmd(q_now, 0.8); // auto joint_cmd = buildRightArmJointCmd(q_now, 0.8);
ASSERT_NO_THROW(robot->servoJ(joint_cmd, 0.8, 0.01)); ASSERT_NO_THROW(robot->servoJ(cmd, 0.8, 0.01));
while (true) { while (true) {
std::this_thread::sleep_for(std::chrono::milliseconds(10000)); std::this_thread::sleep_for(std::chrono::milliseconds(10000));
} }
@ -149,13 +149,13 @@ TEST(HumanoidRobotTest,ServoJAndGetJointQSmokeTest) {
} }
TEST(HumanoidRobotTest,IBVSWithRealRobot) { TEST(HumanoidRobotTest,IBVSWithRealRobot) {
const XmlNode config("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml"); const XmlNode config("/home/lgv/cmvr/cmvr-es/cmvr-es/common/config/cabin_robot.xml");
ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found"; ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found";
auto dmgr_cfg = config.getChild("DeviceManager"); auto dmgr_cfg = config.getChild("DeviceManager");
auto& dmgr = DeviceManager::getInstance(dmgr_cfg); auto& dmgr = DeviceManager::getInstance(dmgr_cfg);
auto robot = dmgr.getDevice<AbstractRobot>("hc01"); auto robot = dmgr.getDevice<AbstractRobot>("hc01");
auto camera = dmgr.getDevice<AbstractCamera>("cam4"); auto camera = dmgr.getDevice<AbstractCamera>("cam2");
ASSERT_NE(robot, nullptr); ASSERT_NE(robot, nullptr);
ASSERT_NE(camera, nullptr); ASSERT_NE(camera, nullptr);
@ -189,7 +189,7 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) {
ibvs_controller.setDepthZGain(1.0); ibvs_controller.setDepthZGain(1.0);
ASSERT_TRUE(ibvs_controller.init(camera, ASSERT_TRUE(ibvs_controller.init(camera,
"/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.urdf", "/home/lgv/cmvr/cmvr-es/model/xiaoyan_description/dual_arm.urdf",
"PELVIS_S", "PELVIS_S",
"R_WRIST_R_S", "R_WRIST_R_S",
"R_CAM")) "R_CAM"))
@ -213,8 +213,8 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) {
<< "Failed to extract right-arm 7 joints from robot state"; << "Failed to extract right-arm 7 joints from robot state";
ibvs_controller.reset(q_now); ibvs_controller.reset(q_now);
const int max_steps = 300; const int max_steps = 30000;
const int log_every = 10; const int log_every = 1;
const int cycle_ms = 33; const int cycle_ms = 33;
int ok_steps = 0; int ok_steps = 0;
@ -252,7 +252,7 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) {
} }
auto joint_cmd = buildRightArmJointCmd(q_cmd_next, 0.8); auto joint_cmd = buildRightArmJointCmd(q_cmd_next, 0.8);
robot->servoJ(joint_cmd, 0.8, dt); // robot->servoJ(joint_cmd, 0.8, dt);
++ok_steps; ++ok_steps;
if ((step % log_every) == 0) { if ((step % log_every) == 0) {

49
cmvr-es/perception/CMakeLists.txt
View File

@ -1,4 +1,4 @@
find_package(VISP REQUIRED) #find_package(VISP REQUIRED)
find_package(OpenCV REQUIRED) find_package(OpenCV REQUIRED)
add_library(perception SHARED add_library(perception SHARED
@ -7,24 +7,41 @@ add_library(perception SHARED
target_include_directories(perception PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) target_include_directories(perception PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
target_link_libraries(perception PUBLIC target_link_libraries(perception PUBLIC
${VISP_LIBRARIES} visp_vs
visp_visual_features
visp_vision
visp_tt_mi
visp_tt
visp_me
visp_mbt
visp_klt
visp_dnn_tracker
visp_blob
visp_sensor
visp_robot
visp_io
visp_imgproc
visp_gui
visp_detection
visp_core
visp_ar
${OpenCV_LIBS} ${OpenCV_LIBS}
) )
add_library(cmvr_es::perception ALIAS perception) add_library(cmvr_es::perception ALIAS perception)
install(TARGETS perception LIBRARY DESTINATION lib) install(TARGETS perception LIBRARY DESTINATION lib)
add_executable(tag_relative_target_3d_test #add_executable(tag_relative_target_3d_test
src/tag_relative_target_3d_test.cpp # src/tag_relative_target_3d_test.cpp
) #)
#
target_link_libraries(tag_relative_target_3d_test #target_link_libraries(tag_relative_target_3d_test
PRIVATE # PRIVATE
cmvr_es::perception # cmvr_es::perception
cmvr_es::device::realsense_camera # cmvr_es::device::realsense_camera
cmvr_es::proto # cmvr_es::proto
glog # glog
gtest # gtest
gtest_main # gtest_main
pthread # pthread
) #)