From 7d03c0eeaf7c03d1d79b28297a272d349ff11255 Mon Sep 17 00:00:00 2001 From: lgv Date: Mon, 3 Nov 2025 11:44:52 +0800 Subject: [PATCH] feat:add get pose rpc service --- config/cabin_robot.xml | 10 +- .../hc_description/dual_arm.urdf | 2 +- example/robot_wrapper.cpp | 35 ++- include/service/grpc_humanoid_robot_service.h | 3 +- protos/cmvr/api/humanoid_robot_command.proto | 13 + protos/cmvr/api/humanoid_robot_service.proto | 1 + .../get_chessboard_position.cpython-310.pyc | Bin 3139 -> 3139 bytes python/vision_servo/follow_target.py | 277 ++++++++---------- python/vision_servo/robot_warpper_test.py | 25 +- src/devices/motor/ti5_motor/ti5_motor.h | 3 +- .../robot/humanoid_robot/humanoid_robot.cpp | 1 + .../humanoid_robot/humanoid_robot_test.cpp | 14 +- src/service/grpc_humanoid_robot_service.cpp | 34 +++ 13 files changed, 243 insertions(+), 175 deletions(-) diff --git a/config/cabin_robot.xml b/config/cabin_robot.xml index 54ae96de..0a620a29 100644 --- a/config/cabin_robot.xml +++ b/config/cabin_robot.xml @@ -20,10 +20,10 @@ - + - + @@ -40,7 +40,7 @@ bufferSize="50" verbose="false"> - + @@ -58,7 +58,7 @@ - + @@ -94,7 +94,7 @@ - + diff --git a/config/robot_description/hc_description/dual_arm.urdf b/config/robot_description/hc_description/dual_arm.urdf index d94be661..293d1228 100644 --- a/config/robot_description/hc_description/dual_arm.urdf +++ b/config/robot_description/hc_description/dual_arm.urdf @@ -574,7 +574,7 @@ - + diff --git a/example/robot_wrapper.cpp b/example/robot_wrapper.cpp index 49f2019f..30302d6c 100644 --- a/example/robot_wrapper.cpp +++ b/example/robot_wrapper.cpp @@ -85,6 +85,23 @@ public: return values; } + void move(const std::string &base_link, const std::string &ee_link, + double x, double y, double z, double rx, double ry, double rz, + double vel, double acc) { + checkRobotInit(); + + cmvr::msgs::Pose3d pose; + pose.mutable_position()->set_x(x); + pose.mutable_position()->set_y(y); + pose.mutable_position()->set_z(z); + + pose.mutable_euler()->set_rx(rx); + pose.mutable_euler()->set_ry(ry); + pose.mutable_euler()->set_rz(rz); + + robot_->moveJ(base_link, ee_link, pose,vel,acc); + } + std::vector ik(const std::string& side, const std::string &base_link, const std::string &ee_link, @@ -117,6 +134,15 @@ public: } } + + std::tuple fk(const std::string &base_link, const std::string &ee_link) { + checkRobotInit(); + auto pose = robot_->fk(base_link, ee_link); + + // 返回一个元组，包含 (x, y, z, rx, ry, rz) + return std::make_tuple(pose.position().x(), pose.position().y(), pose.position().z(), pose.euler().rx(), pose.euler().ry(), pose.euler().ry()); + } + private: static DeviceManager* dmgr_; static std::shared_ptr robot_; @@ -166,5 +192,12 @@ PYBIND11_MODULE(robot_wrapper, m) { py::arg("side"), py::arg("base_link"), py::arg("ee_link"), py::arg("x"), py::arg("y"), py::arg("z"), py::arg("rx"), py::arg("ry"), py::arg("rz"), - "Compute inverse kinematics and return 7 joint values for the given side"); + "Compute inverse kinematics and return 7 joint values for the given side") + .def("moveJ", &PyRobotWrapper::move, "Move robot in joint space", + py::arg("base_link"), py::arg("ee_link"), + py::arg("x"), py::arg("y"), py::arg("z"), + py::arg("rx"), py::arg("ry"), py::arg("rz"), + py::arg("vel") = 0.5, py::arg("acc") = 0.1) + .def("fk", &PyRobotWrapper::fk, "Compute forward kinematics", + py::arg("base_link"), py::arg("ee_link")); } diff --git a/include/service/grpc_humanoid_robot_service.h b/include/service/grpc_humanoid_robot_service.h index 498522a1..04b4ee3a 100644 --- a/include/service/grpc_humanoid_robot_service.h +++ b/include/service/grpc_humanoid_robot_service.h @@ -5,7 +5,7 @@ #pragma once #include "device_manager/device_manager.h" -#include "cmvr/api/humanoid_robot.grpc.pb.h" +#include "cmvr/api/humanoid_robot_service.grpc.pb.h" namespace cmvr { namespace service { @@ -27,6 +27,7 @@ namespace cmvr { grpc::Status speedL(grpc::ServerContext* context, const cmvr::api::SpeedL_Request* request, cmvr::api::SpeedL_Response* response) override; grpc::Status getJointState(grpc::ServerContext *context, const cmvr::api::JointRequest *request, cmvr::api::JointResponse *response) override; + grpc::Status getPose(grpc::ServerContext *context, const cmvr::api::GetPose_Request *request, cmvr::api::GetPose_Response *response) override; private: device::DeviceManager& dmgr_; }; diff --git a/protos/cmvr/api/humanoid_robot_command.proto b/protos/cmvr/api/humanoid_robot_command.proto index a9c909a4..d3554373 100644 --- a/protos/cmvr/api/humanoid_robot_command.proto +++ b/protos/cmvr/api/humanoid_robot_command.proto @@ -54,6 +54,19 @@ message MoveJ{ } +message GetPose{ + message Request{ + CommandHeader.Request header = 1; + string base_link = 2; + string ee_link = 3; + } + + message Response{ + CommandHeader.Feedback header= 1; + Pose3D pose = 2; + } +} + message MoveL{ message Request{ diff --git a/protos/cmvr/api/humanoid_robot_service.proto b/protos/cmvr/api/humanoid_robot_service.proto index fafb6726..8438ffbc 100644 --- a/protos/cmvr/api/humanoid_robot_service.proto +++ b/protos/cmvr/api/humanoid_robot_service.proto @@ -13,4 +13,5 @@ service HumanoidRobotService{ rpc speedJ(SpeedJ.Request) returns (SpeedJ.Response); rpc speedL(SpeedL.Request) returns (SpeedL.Response); rpc getJointState(JointRequest) returns (JointResponse); + rpc getPose(GetPose.Request) returns (GetPose.Response); } diff --git a/python/vision_servo/__pycache__/get_chessboard_position.cpython-310.pyc b/python/vision_servo/__pycache__/get_chessboard_position.cpython-310.pyc index 4ded120991efaea0e7fccb80fd5a71ef0eef827b..9dcaa67232ff05285946083bd91fd2df24aa3628 100644 GIT binary patch delta 20 acmX>saae*opO=@50SHdjyx7QX!2saae*opO=@50SF$>-M5k3f(HOPS_P2+ diff --git a/python/vision_servo/follow_target.py b/python/vision_servo/follow_target.py index 3478c4f5..60788d5c 100644 --- a/python/vision_servo/follow_target.py +++ b/python/vision_servo/follow_target.py @@ -326,170 +326,147 @@ def compute_point_in_base(T_base_ee, R_ee_cam, t_ee_cam, P_cam_target): return P_base_target -if __name__ == "__main__": - pipeline, align = init_realsense() - try: - # 初始化机器人 - robot = Robot("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml", "hc01") - - # 移动到初始关节位姿 - - init_joint = [-0.304106 , 1.30538 , 1.4465 ,1.93647 , -2.84955 ,-0.116586 ,0.123911] - # init_joint = [0 ,0 , 0 ,0 , 0 ,0 ,0] - robot.moveJ("right", init_joint) - - js = robot.getJointQ('right') - print("当前关节角:", js) - - # 获取末端在基座下的位姿 - # T_base_ee = get_pose(js, base_link="PELVIS_S", target_link="R_FINGER_TIP") - T_base_ee = get_pose(js, base_link="PELVIS_S", target_link="R_FINGER_TIP") - print("末端位姿 T_base_ee:\n", T_base_ee) - # T_base_ee = get_pose(js, base_link="PELVIS_S", target_link="R_FINGER_TIP") - # print("末端位姿 T_base_ee:\n", T_base_ee) - # - # T_base_ee = get_pose(js, base_link="R_WRIST_R_S", target_link="R_FINGER_TIP") - # print("末端位姿 T_base_ee:\n", T_base_ee) - - # while True: - # time.sleep(1) - - - - # 获取红点在相机坐标系下的坐标 - P_cam_target = get_closest_red_point(pipeline, align, vis_path="red_point.png") - if P_cam_target is None: - raise RuntimeError("未检测到红点，无法计算目标点") - print("红点在相机坐标系下:", P_cam_target) - - # P_cam_target = np.array([0.0716422, 0.0482324, 0.325]) - - # "--frame-id", - # "R_FINGER_TIP", - # "--child-frame-id", - # "camera_color_optical_frame", - # "--x", - # "-0.132714", - # "--y", - # "0.00161894", - # "--z", - # "0.0760576", - # "--qx", - # "0.512312", - # "--qy", - # "-0.503306", - # "--qz", - # "0.493563", - # "--qw", - # "-0.490525", - # # "--roll", - # # "0.18103", - # # "--pitch", - # # "1.60289", - # # "--yaw", - # # "-1.76387", - - # 手眼标定结果：四元数和平移 - # q_ee_cam = np.array([0.519375,-0.502778, 0.4866,-0.490596]) - # t_ee_cam = np.array([-0.0815577, 0.0321285, 0.0746921]) - - q_ee_cam = np.array([0.512312, -0.503306, 0.493563,-0.490525]) - t_ee_cam = np.array([-0.124714, -0.02281894, 0.0800576]) - R_ee_cam = R.from_quat(q_ee_cam).as_matrix() - print("末端->相机旋转矩阵 R_ee_cam:\n", R_ee_cam) - - - # 构造相机相对于 末端的变换矩阵 - T_ee_cam = np.eye(4) - T_ee_cam[:3, :3] = R_ee_cam - T_ee_cam[:3, 3] = t_ee_cam - - # 相机相对于 基座的 变换矩阵 - T_base_cam = T_base_ee @ T_ee_cam - - # 相机下红点 -> 基座下红点 - P_base_target = np.append(P_cam_target, 1) # 齐次坐标 - P_base_target = (T_base_cam @ P_base_target)[:3] - print("红点在基座坐标系下:", P_base_target) - - - # while True: - # time.sleep(1) - - # ----------------- IK 求解 ----------------- - # 目标位姿 为 相机 相对于 基座的 姿态 - # R_base_target = T_base_ee[:3, :3] @ R_ee_cam - R_base_target = T_base_ee[:3, :3] - rot = R.from_matrix(R_base_target) - target_euler = rot.as_euler('xyz', degrees=True) - d = rot.as_euler('xyz', degrees=False) - target_rx, target_ry, target_rz = target_euler - quat = rot.as_quat() - print("目标旋转欧拉角 (XYZ 度):", target_euler) - print("目标旋转欧拉角 (XYZ 弧度):", d) - print("目标旋转四元数 [x,y,z,w]:", quat) - - # while True: - # time.sleep(10) - - # IK 求解 - ok, lq, rq = solve_ik(*P_base_target, target_rx, target_ry, target_rz, - base_link="PELVIS_S", target_link="R_FINGER_TIP") - if not ok: - raise RuntimeError("IK 求解失败") - - # 移动到目标点 - robot.moveJ("right", rq) - print("已到达目标点，按 Ctrl+C 回到初始位姿...") - - while True: - time.sleep(1) - - except Exception as e: - print("捕获异常:", e) - - finally: - # 回到初始位姿 - robot.moveJ("right", [-0.05804541534555635, 1.460164607187404, 1.458934384971377, 0.29042831678163805, - -1.498103103566999, 0.039690803641003906, -0.08653132466712826]) - pipeline.stop() - align = None - - # if __name__ == "__main__": -# +# pipeline, align = init_realsense() # try: # # 初始化机器人 # robot = Robot("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml", "hc01") +# +# # 移动到初始关节位姿 +# +# init_joint = [-0.304106 , 1.30538 , 1.4465 ,1.93647 , -2.84955 ,-0.116586 ,0.123911] +# # init_joint = [0 ,0 , 0 ,0 , 0 ,0 ,0] +# robot.moveJ("right", init_joint) +# # js = robot.getJointQ('right') -# # js = np.zeros(7) -# # js[6] = 0.8 +# print("当前关节角:", js) # -# T_base_ee = get_pose(js, base_link="PELVIS_S", target_link="R_WRIST_R_S") -# T_base_cam = get_pose(js, base_link="PELVIS_S", target_link="R_FINGER_TIP") +# # 获取末端在基座下的位姿 +# # T_base_ee = get_pose(js, base_link="PELVIS_S", target_link="R_FINGER_TIP") +# T_base_ee = get_pose(js, base_link="PELVIS_S", target_link="R_FINGER_TIP") +# print("末端位姿 T_base_ee:\n", T_base_ee) +# # T_base_ee = get_pose(js, base_link="PELVIS_S", target_link="R_FINGER_TIP") +# # print("末端位姿 T_base_ee:\n", T_base_ee) +# # +# # T_base_ee = get_pose(js, base_link="R_WRIST_R_S", target_link="R_FINGER_TIP") +# # print("末端位姿 T_base_ee:\n", T_base_ee) # -# print(T_base_cam - T_base_ee) -# T_ee1_ee2 = get_pose(js, base_link="R_FINGER_TIP", target_link="R_WRIST_R_S") -# print(T_ee1_ee2) +# # while True: +# # time.sleep(1) # # -# T_ee2_ee1 = get_pose(js, base_link="R_WRIST_R_S", target_link="R_FINGER_TIP") -# print(T_ee2_ee1) +# +# # 获取红点在相机坐标系下的坐标 +# P_cam_target = get_closest_red_point(pipeline, align, vis_path="red_point.png") +# if P_cam_target is None: +# raise RuntimeError("未检测到红点，无法计算目标点") +# print("红点在相机坐标系下:", P_cam_target) +# +# # P_cam_target = np.array([0.0716422, 0.0482324, 0.325]) +# +# # "--frame-id", +# # "R_FINGER_TIP", +# # "--child-frame-id", +# # "camera_color_optical_frame", +# # "--x", +# # "-0.132714", +# # "--y", +# # "0.00161894", +# # "--z", +# # "0.0760576", +# # "--qx", +# # "0.512312", +# # "--qy", +# # "-0.503306", +# # "--qz", +# # "0.493563", +# # "--qw", +# # "-0.490525", +# # # "--roll", +# # # "0.18103", +# # # "--pitch", +# # # "1.60289", +# # # "--yaw", +# # # "-1.76387", +# +# # 手眼标定结果：四元数和平移 +# # q_ee_cam = np.array([0.519375,-0.502778, 0.4866,-0.490596]) +# # t_ee_cam = np.array([-0.0815577, 0.0321285, 0.0746921]) +# +# q_ee_cam = np.array([0.512312, -0.503306, 0.493563,-0.490525]) +# t_ee_cam = np.array([-0.124714, -0.02281894, 0.0800576]) +# R_ee_cam = R.from_quat(q_ee_cam).as_matrix() +# print("末端->相机旋转矩阵 R_ee_cam:\n", R_ee_cam) # # -# # robot.moveJ("right", [0.00203898, 1.34062, 0.0,0.322261, 0.0,-0.000210733, -0.0942364]) +# # 构造相机相对于 末端的变换矩阵 +# T_ee_cam = np.eye(4) +# T_ee_cam[:3, :3] = R_ee_cam +# T_ee_cam[:3, 3] = t_ee_cam # -# # robot.moveJ("right", [-0.344938, 0.935147, 2.27031,1.68959, -2.32841,0.460145, 0.300996]) -# # js = robot.getJointQ('right') -# # print("js = ") -# # print(js) +# # 相机相对于 基座的 变换矩阵 +# T_base_cam = T_base_ee @ T_ee_cam +# +# # 相机下红点 -> 基座下红点 +# P_base_target = np.append(P_cam_target, 1) # 齐次坐标 +# P_base_target = (T_base_cam @ P_base_target)[:3] +# print("红点在基座坐标系下:", P_base_target) # # +# # while True: +# # time.sleep(1) +# +# # ----------------- IK 求解 ----------------- +# # 目标位姿 为 相机 相对于 基座的 姿态 +# # R_base_target = T_base_ee[:3, :3] @ R_ee_cam +# R_base_target = T_base_ee[:3, :3] +# rot = R.from_matrix(R_base_target) +# target_euler = rot.as_euler('xyz', degrees=True) +# d = rot.as_euler('xyz', degrees=False) +# target_rx, target_ry, target_rz = target_euler +# quat = rot.as_quat() +# print("目标旋转欧拉角 (XYZ 度):", target_euler) +# print("目标旋转欧拉角 (XYZ 弧度):", d) +# print("目标旋转四元数 [x,y,z,w]:", quat) +# +# # while True: +# # time.sleep(10) +# +# # IK 求解 +# ok, lq, rq = solve_ik(*P_base_target, target_rx, target_ry, target_rz, +# base_link="PELVIS_S", target_link="R_FINGER_TIP") +# if not ok: +# raise RuntimeError("IK 求解失败") +# +# # 移动到目标点 +# robot.moveJ("right", rq) # print("已到达目标点，按 Ctrl+C 回到初始位姿...") +# # while True: # time.sleep(1) -# except SystemExit: -# print("安全退出程序...") -# # -# # finally: -# # robot.moveJ("right", [0.00203898, 1.34062, 0.0,0.322261, 0.0,-0.000210733, -0.0942364]) +# +# except Exception as e: +# print("捕获异常:", e) +# +# finally: +# # 回到初始位姿 +# robot.moveJ("right", [-0.05804541534555635, 1.460164607187404, 1.458934384971377, 0.29042831678163805, +# -1.498103103566999, 0.039690803641003906, -0.08653132466712826]) +# pipeline.stop() +# align = None + + +if __name__ == "__main__": + + + # 初始化机器人 + robot = Robot("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml", "hc01") + # js = robot.getJointQ('right') + js = np.zeros(7) + js = np.array([-0.054383226063586795, 1.4797969033848364, -1.4685445467885516, -0.03790717127687202, -0.2797606954361809, 0.1916896945079446, -0.17680932146902423]) + # js[6] = 0.8 + + T_base_ee = get_pose(js, base_link="PELVIS_S", target_link="R_WRIST_R_S") + # T_base_cam = get_pose(js, base_link="PELVIS_S", target_link="R_FINGER_TIP") + print(T_base_ee) + diff --git a/python/vision_servo/robot_warpper_test.py b/python/vision_servo/robot_warpper_test.py index 8369c10a..90de3bb9 100644 --- a/python/vision_servo/robot_warpper_test.py +++ b/python/vision_servo/robot_warpper_test.py @@ -10,33 +10,38 @@ sys.path.append("/home/lgv/cmvr/cmvr-es/cmake-build-debug/example") # 导入模块 from robot_wrapper import Robot +# R_WRIST_R_S # 初始化机器人（传入配置文件路径和机器人名称） robot = Robot("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml", "hc01") - - - - +# robot.torqueOn() +# +# +# x,y,z,rx,ry,rz = robot.fk(base_link="PELVIS_S", ee_link="R_FINGER_TIP") +# # robot->move(base_link="PELVIS_S", target_link="R_FINGER_TIP") +# +# +# print(x, y, z, rx, ry, rz, sep=',') # # # while True: # for j in JOINT_POSITIONS: # robot.moveJ('right', j) -# robot.torqueOn() + # time.sleep(10) # js = robot.getJointQ('right') # print(js) # 控制左臂关节 -# robot.calibrateZeroQ("R_WRIST_Y") +robot.calibrateZeroQ("R_WRIST_Y") # robot.calibrateZeroQ("R_WRIST_R") -# time.sleep(3) -robot.moveJ("right", [0.0, 0.0, 0.0, 0.0, 0.0,0.0, 0.0]) +time.sleep(3) +# robot.moveJ("right", [0.0, 0.0, 0.0, 0.0, 0.0,0.0, 0.0]) # # robot.moveJ("right", [-0.344938, 0.935147, 2.27031,1.68959, -2.32841,0.460145, 0.300996]) # robot.moveJ("right", [0.0, 0.0, 0.0, 0.0, 0.0,0.0, 0.0]) # # -time.sleep(10) +# time.sleep(10) -# # robot.torqueOff("R_WRIST_R") +# robot.torqueOff("R_WRIST_R") # time.sleep(10) # robot.moveJ("right", [0.0, 0.0, 0.0, 0.0, 0.0,0.0, 0.0]) # time.sleep(20) diff --git a/src/devices/motor/ti5_motor/ti5_motor.h b/src/devices/motor/ti5_motor/ti5_motor.h index 12a35b63..e3765f8c 100644 --- a/src/devices/motor/ti5_motor/ti5_motor.h +++ b/src/devices/motor/ti5_motor/ti5_motor.h @@ -32,11 +32,12 @@ namespace cmvr { if (protocol_->comm_proto == MotorProtocolInterface::CommProto::CANOPEN ) { auto canopen_protocol = std::dynamic_pointer_cast(protocol_); canopen_protocol->configPdo(node_id_); - canopen_protocol->configProfile(node_id_,2000,2000,2000); + canopen_protocol->configProfile(node_id_,4000,8000,8000); canopen_protocol->seedNmtRequest(node_id_,msgs::NMT_ENTER_PRE_OPERATIONAL); canopen_protocol->seedNmtRequest(node_id_,msgs::NMT_START_REMOTE_NODE); canopen_protocol->seedSdoRequest(node_id_, msgs::CS_WRITE_TWO_BYTES, msgs::CONTROL_WORD_6040, msgs::SUB_INDEX_0, 0x06); canopen_protocol->seedSdoRequest(node_id_, msgs::CS_WRITE_TWO_BYTES, msgs::CONTROL_WORD_6040, msgs::SUB_INDEX_0, 0x0F); + canopen_protocol->setLimitQd(node_id_,6.0); } } }; diff --git a/src/devices/robot/humanoid_robot/humanoid_robot.cpp b/src/devices/robot/humanoid_robot/humanoid_robot.cpp index 22ce27e6..385d4bf2 100644 --- a/src/devices/robot/humanoid_robot/humanoid_robot.cpp +++ b/src/devices/robot/humanoid_robot/humanoid_robot.cpp @@ -1587,6 +1587,7 @@ cmvr::msgs::Pose3d HumanoidRobot::fk(const std::string &base_link, const st q_map["R_SHOULDER_P"], q_map["R_SHOULDER_R"], q_map["R_SHOULDER_Y"], q_map["R_ELBOW_R"], q_map["R_WRIST_P"], q_map["R_WRIST_Y"], q_map["R_WRIST_R"]; + LOG(INFO) << "q_current: " << q; // 更新状态并计算前向运动学 m_state_->SetQ(q); m_robot_->ComputeForwardKinematics(m_state_); diff --git a/src/devices/robot/humanoid_robot/humanoid_robot_test.cpp b/src/devices/robot/humanoid_robot/humanoid_robot_test.cpp index 8dd19c57..bf5e1913 100644 --- a/src/devices/robot/humanoid_robot/humanoid_robot_test.cpp +++ b/src/devices/robot/humanoid_robot/humanoid_robot_test.cpp @@ -443,12 +443,14 @@ TEST(HumanoidRobotTest, MoveIKTest) { auto cur_pose = robot->fk("PELVIS_S","R_FINGER_TIP"); cmvr::msgs::Pose3d pose; - pose.mutable_position()->set_x(0.591101); - pose.mutable_position()->set_y(-0.159451); - pose.mutable_position()->set_z(-0.206554); - pose.mutable_euler()->set_rx(0.070589); - pose.mutable_euler()->set_ry(0.117212); - pose.mutable_euler()->set_rz(0.0916384); + pose.mutable_position()->set_x(0.047436); + pose.mutable_position()->set_y(-0.2884); + pose.mutable_position()->set_z(-0.605183); + pose.mutable_euler()->set_rx(-1.191976); + pose.mutable_euler()->set_ry(1.397317); + pose.mutable_euler()->set_rz(2.721208); + // Pose: {'x': 0.047436, 'y': -0.2884, 'z': -0.605183, 'rx': -1.191976, 'ry': 1.397317, 'rz': 2.721208} + // 打印关节状态线程 std::thread joint_state_thread([&]() { diff --git a/src/service/grpc_humanoid_robot_service.cpp b/src/service/grpc_humanoid_robot_service.cpp index b7797557..4326c3f2 100644 --- a/src/service/grpc_humanoid_robot_service.cpp +++ b/src/service/grpc_humanoid_robot_service.cpp @@ -153,6 +153,40 @@ grpc::Status gRPCHumanoidRobotServiceImpl::speedL(grpc::ServerContext* context, return ret; } +grpc::Status gRPCHumanoidRobotServiceImpl::getPose(grpc::ServerContext *context, + const cmvr::api::GetPose_Request *request, cmvr::api::GetPose_Response *response) { + + grpc::Status ret = grpc::Status::OK; + + try { + auto robot = dmgr_.getDevice(request->header().device_id()); + + std::vector states{}; + auto pose = robot->fk(request->base_link(),request->ee_link()); + + response->mutable_header()->set_success(true); + response->mutable_header()->set_error_message(""); + response->mutable_pose()->set_x(pose.position().x()); + response->mutable_pose()->set_y(pose.position().y()); + response->mutable_pose()->set_z(pose.position().z()); + response->mutable_pose()->set_rx(pose.euler().rx()); + response->mutable_pose()->set_ry(pose.euler().ry()); + response->mutable_pose()->set_rz(pose.euler().rz()); + + + + }catch (const std::exception& e) { + response->mutable_header()->set_success(false); + response->mutable_header()->set_error_message(e.what()); + ret = grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } + *response->mutable_header()->mutable_timestamp() = TimeUtil::GetCurrentTime(); + return ret; + + +} + + grpc::Status gRPCHumanoidRobotServiceImpl::getJointState(grpc::ServerContext *context, const cmvr::api::JointRequest *request, cmvr::api::JointResponse *response) {