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" />-->
<!-- <RightArm />-->
<!-- <Neck/>-->
<!-- <Humanoid id="hc01" dof="14"-->
<!-- urdf="/home/lgv/cmvr/cmvr-es/config/robot_description/hc_description/dual_arm.urdf"-->
<!-- 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"-->
<!-- 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"-->
<!-- verbose="false">-->
<!-- <CanManger id="" devId="">-->
<!-- <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="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="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="21" 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"/>-->
<!-- </LeftArmCan>-->
<!-- <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="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="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="28" 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"/>-->
<!-- </RightArmCan>-->
<!-- <HeadCan id = " " devId = " " channelId ="2" enable="true">-->
<!-- <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="31" jointName="HEAD_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
<!-- </HeadCan>-->
<!-- <WaistCan id = " " devId = " " channelId ="3" enable="false">-->
<!-- <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"/>-->
<!-- </WaistCan>-->
<!-- </CanManger>-->
<Humanoid id="hc01" dof="14"
urdf="/home/lgv/cmvr/cmvr-es/model/xiaoyan_description/dual_arm.urdf"
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"
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"
verbose="false">
<CanManger id="" devId="">
<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="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="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="28" jointName="L_WRIST_Y" 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>
<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="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="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="21" jointName="R_WRIST_Y" limitQLb="-1.102" limitQUb="1.02" limitQd="3.0"/>
<Motor id="22" jointName="R_WRIST_R" limitQLb="-0.293" limitQUb="1.57079" limitQd="3.0"/>
</RightArmCan>
<HeadCan id = " " devId = " " channelId ="2" enable="true">
<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="31" jointName="HEAD_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
</HeadCan>
<WaistCan id = " " devId = " " channelId ="3" enable="false">
<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"/>
</WaistCan>
</CanManger>
<!-- </Humanoid>-->
</Humanoid>
</Robot>
<BioHead>

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

@ -1,6 +1,6 @@
realsense_cameras {
id: "cam2"
serialNumber: "123456789012"
serialNumber: "243122072252"
width: 1280
height: 720
fps: 30
@ -10,7 +10,7 @@ realsense_cameras {
align_mode: ALIGN_MODE_COLOR
buffer_size: 30
sync: true
enable: false
enable: true
}
uvc_cameras {
@ -23,5 +23,5 @@ uvc_cameras {
camera_mode: CAMERA_MODE_PHOTO
stream_mode: STREAM_MODE_RGB
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>
// 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()
{
char* cwd = ::getcwd(nullptr, 0);
if (!cwd) return {};
std::string s(cwd);
std::free(cwd);
return s + "/config/";
return "/home/lgv/cmvr/cmvr-es/output/bin/config/";
}
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
@ -32,7 +32,24 @@ target_link_libraries(controller PUBLIC
ccd
fcl
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_parsers
)

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

@ -108,7 +108,7 @@ TEST(HumanoidRobotTest,MyRobotTest) {
}
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";
auto dmgr_cfg = config.getChild("DeviceManager");
auto& dmgr = DeviceManager::getInstance(dmgr_cfg);
@ -130,18 +130,18 @@ TEST(HumanoidRobotTest,ServoJAndGetJointQSmokeTest) {
ASSERT_TRUE(refreshRightArmQ()) << "Failed to read right-arm joint q";
// std::vector<JointPoint> cmd{};
// cmd = {
std::vector<JointPoint> cmd{};
cmd = {
// {"R_SHOULDER_P", 0.0},
// {"R_SHOULDER_R", 0.0},
// {"R_SHOULDER_Y", 0.0},
// {"R_ELBOW_R", 0.0},
// {"R_WRIST_P", 0.0},
// {"R_WRIST_Y", 0.0},
// {"R_WRIST_R", 0.0},
// };
auto joint_cmd = buildRightArmJointCmd(q_now, 0.8);
ASSERT_NO_THROW(robot->servoJ(joint_cmd, 0.8, 0.01));
{"R_WRIST_R", 0.2},
};
// auto joint_cmd = buildRightArmJointCmd(q_now, 0.8);
ASSERT_NO_THROW(robot->servoJ(cmd, 0.8, 0.01));
while (true) {
std::this_thread::sleep_for(std::chrono::milliseconds(10000));
}
@ -149,13 +149,13 @@ TEST(HumanoidRobotTest,ServoJAndGetJointQSmokeTest) {
}
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";
auto dmgr_cfg = config.getChild("DeviceManager");
auto& dmgr = DeviceManager::getInstance(dmgr_cfg);
auto robot = dmgr.getDevice<AbstractRobot>("hc01");
auto camera = dmgr.getDevice<AbstractCamera>("cam4");
auto camera = dmgr.getDevice<AbstractCamera>("cam2");
ASSERT_NE(robot, nullptr);
ASSERT_NE(camera, nullptr);
@ -189,7 +189,7 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) {
ibvs_controller.setDepthZGain(1.0);
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",
"R_WRIST_R_S",
"R_CAM"))
@ -213,8 +213,8 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) {
<< "Failed to extract right-arm 7 joints from robot state";
ibvs_controller.reset(q_now);
const int max_steps = 300;
const int log_every = 10;
const int max_steps = 30000;
const int log_every = 1;
const int cycle_ms = 33;
int ok_steps = 0;
@ -252,7 +252,7 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) {
}
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;
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)
add_library(perception SHARED
@ -7,24 +7,41 @@ add_library(perception SHARED
target_include_directories(perception PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
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}
)
add_library(cmvr_es::perception ALIAS perception)
install(TARGETS perception LIBRARY DESTINATION lib)
add_executable(tag_relative_target_3d_test
src/tag_relative_target_3d_test.cpp
)
target_link_libraries(tag_relative_target_3d_test
PRIVATE
cmvr_es::perception
cmvr_es::device::realsense_camera
cmvr_es::proto
glog
gtest
gtest_main
pthread
)
#add_executable(tag_relative_target_3d_test
# src/tag_relative_target_3d_test.cpp
#)
#
#target_link_libraries(tag_relative_target_3d_test
# PRIVATE
# cmvr_es::perception
# cmvr_es::device::realsense_camera
# cmvr_es::proto
# glog
# gtest
# gtest_main
# pthread
#)