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

feat: add grpc debug log

Browse Source
This commit is contained in:
lgv 2026-06-25 11:03:38 +08:00
parent 693fa0ab76
commit c7f31708cb
28 changed files with 277 additions and 90 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
cmvr-es/algorithms/controllers/arm_control/include/cartesian_velocity_controller.h
View File

@ -8,6 +8,7 @@
#include <functional>
#include <memory>
#include <mutex>
#include <optional>
#include <thread>
#include <vector>
@ -42,7 +43,7 @@ public:
double acceleration,
double duration,
FrameType frame);
Result stop(double acceleration);
Result stop(std::optional<double> acceleration = std::nullopt);
void shutdown();
bool busy() const { return busy_.load(); }

6
cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller.cpp
View File

@ -95,9 +95,8 @@ Result CartesianVelocityController::speedL(const CartesianVelocity& velocity,
return Result::success();
}
Result CartesianVelocityController::stop(const double acceleration)
Result CartesianVelocityController::stop(const std::optional<double> acceleration)
{
(void)acceleration;
if (!worker_ || !worker_->joinable()) {
return Result::success();
}
@ -105,6 +104,9 @@ Result CartesianVelocityController::stop(const double acceleration)
std::lock_guard<std::mutex> lock(mutex_);
target_twist_ = {};
target_frame_ = FrameType::Base;
if (acceleration.has_value()) {
target_acceleration_ = *acceleration;
}
command_active_ = true;
++command_version_;
}

12
cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_dls_ik_solver.cpp
View File

@ -101,16 +101,16 @@ bool PinocchioDlsIKSolver::refreshJointLimits_(const config::PinocchioDlsIKConfi
return false;
}
const auto& limit = it->second;
if (!std::isfinite(limit.lower()) || !std::isfinite(limit.upper()) ||
!std::isfinite(limit.velocity()) || limit.upper() <= limit.lower() ||
limit.velocity() <= 0.0) {
if (!std::isfinite(limit.q_lb()) || !std::isfinite(limit.q_ub()) ||
!std::isfinite(limit.qd()) || limit.q_ub() <= limit.q_lb() ||
limit.qd() <= 0.0) {
CMVR_LOG(ERROR) << "[PinocchioDlsIKSolver] invalid custom joint limit for "
<< limit.joint_name();
return false;
}
joint_pos_lower_limits_[i] = limit.lower();
joint_pos_upper_limits_[i] = limit.upper();
joint_vel_limits_[i] = std::abs(limit.velocity());
joint_pos_lower_limits_[i] = limit.q_lb();
joint_pos_upper_limits_[i] = limit.q_ub();
joint_vel_limits_[i] = std::abs(limit.qd());
}
return true;
}

12
cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_qp/src/pinocchio_qp_cartesian_motion_planner.cpp
View File

@ -94,16 +94,16 @@ bool PinocchioQpCartesianMotionPlanner::refreshJointLimits_(
return false;
}
const auto& limit = it->second;
if (!std::isfinite(limit.lower()) || !std::isfinite(limit.upper()) ||
!std::isfinite(limit.velocity()) || limit.upper() <= limit.lower() ||
limit.velocity() <= 0.0) {
if (!std::isfinite(limit.q_lb()) || !std::isfinite(limit.q_ub()) ||
!std::isfinite(limit.qd()) || limit.q_ub() <= limit.q_lb() ||
limit.qd() <= 0.0) {
CMVR_LOG(ERROR) << "[PinocchioQpCartesianMotionPlanner] invalid custom joint limit for "
<< limit.joint_name();
return false;
}
joint_lower_limits_[i] = limit.lower();
joint_upper_limits_[i] = limit.upper();
joint_velocity_limits_[i] = std::abs(limit.velocity());
joint_lower_limits_[i] = limit.q_lb();
joint_upper_limits_[i] = limit.q_ub();
joint_velocity_limits_[i] = std::abs(limit.qd());
}
return true;
}

31
cmvr-es/config/devices/camera/camera.pb.txt
View File

@ -63,19 +63,22 @@ camera {
}
}
cameras {
id: "cam5"
realsense {
serialNumber: "243122075389"
width: 640
height: 480
fps: 30
codec: "H265"
camera_mode: CAMERA_MODE_VIDEO
stream_mode: STREAM_MODE_RGBD
align_mode: ALIGN_MODE_COLOR
buffer_size: 30
sync: false
cameras {
id: "cam5"
realsense {
serialNumber: "243122070435"
width: 1280
height: 720
encode_width: 640
encode_height: 360
fps: 30
codec: "H264"
camera_mode: CAMERA_MODE_VIDEO
stream_mode: STREAM_MODE_RGB
align_mode: ALIGN_MODE_COLOR
buffer_size: 30
sync: false
}
}
}
}

26
cmvr-es/config/devices/motor/ti5_motors.pb.txt
View File

@ -10,8 +10,8 @@ motor {
}
joint_limits {
source: JOINT_LIMIT_SOURCE_URDF
urdf_path: "model/xiaoyan_description/dual_arm.urdf"
}
joint_limits_urdf_path: "model/xiaoyan_description/dual_arm.urdf"
motors { id: 23 joint_name: "L_SHOULDER_P" }
motors { id: 24 joint_name: "L_SHOULDER_R" }
motors { id: 25 joint_name: "L_SHOULDER_Y" }
@ -30,13 +30,13 @@ motor {
}
joint_limits {
source: JOINT_LIMIT_SOURCE_CUSTOM
joints { joint_name: "R_SHOULDER_P" lower: -3.14 upper: 3.14 velocity: 5.0 }
joints { joint_name: "R_SHOULDER_R" lower: -0.78 upper: 1.57 velocity: 5.0 }
joints { joint_name: "R_SHOULDER_Y" lower: -3.14 upper: 3.14 velocity: 5.0 }
joints { joint_name: "R_ELBOW_R" lower: 0 upper: 2.05 velocity: 5.0 }
joints { joint_name: "R_WRIST_P" lower: -3.14 upper: 3.14 velocity: 5.0 }
joints { joint_name: "R_WRIST_Y" lower: -0.78 upper: 0.78 velocity: 5.0 }
joints { joint_name: "R_WRIST_R" lower: -0.57 upper: 1.57 velocity: 5.0 }
joints { joint_name: "R_SHOULDER_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
joints { joint_name: "R_SHOULDER_R" q_lb: -0.78 q_ub: 1.57 qd: 5.0 qdd: 10.0 }
joints { joint_name: "R_SHOULDER_Y" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
joints { joint_name: "R_ELBOW_R" q_lb: 0 q_ub: 2.05 qd: 5.0 qdd: 10.0 }
joints { joint_name: "R_WRIST_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
joints { joint_name: "R_WRIST_Y" q_lb: -0.78 q_ub: 0.78 qd: 5.0 qdd: 10.0 }
joints { joint_name: "R_WRIST_R" q_lb: -0.57 q_ub: 1.57 qd: 5.0 qdd: 10.0 }
}
motors { id: 16 joint_name: "R_SHOULDER_P" }
motors { id: 17 joint_name: "R_SHOULDER_R" }
@ -55,9 +55,9 @@ motor {
}
joint_limits {
source: JOINT_LIMIT_SOURCE_CUSTOM
joints { joint_name: "HEAD_Y" lower: -3.14 upper: 3.14 velocity: 3.0 }
joints { joint_name: "HEAD_P" lower: -3.14 upper: 3.14 velocity: 3.0 }
joints { joint_name: "HEAD_R" lower: -3.14 upper: 3.14 velocity: 3.0 }
joints { joint_name: "HEAD_Y" q_lb: -3.14 q_ub: 3.14 qd: 3.0 }
joints { joint_name: "HEAD_P" q_lb: -3.14 q_ub: 3.14 qd: 3.0 }
joints { joint_name: "HEAD_R" q_lb: -3.14 q_ub: 3.14 qd: 3.0 }
}
motors { id: 32 joint_name: "HEAD_Y" }
motors { id: 30 joint_name: "HEAD_P" }
@ -72,8 +72,8 @@ motor {
}
joint_limits {
source: JOINT_LIMIT_SOURCE_CUSTOM
joints { joint_name: "WAIST_Y" lower: -3.14 upper: 3.14 velocity: 3.0 }
joints { joint_name: "WAIST_P" lower: -3.14 upper: 3.14 velocity: 3.0 }
joints { joint_name: "WAIST_Y" q_lb: -3.14 q_ub: 3.14 qd: 3.0 }
joints { joint_name: "WAIST_P" q_lb: -3.14 q_ub: 3.14 qd: 3.0 }
}
motors { id: 4 joint_name: "WAIST_Y" }
motors { id: 15 joint_name: "WAIST_P" }

8
cmvr-es/config/logger/logger.pb.txt
View File

@ -3,7 +3,7 @@ logger {
routes {
level: LOG_LEVEL_DEBUG
file: true
file: false
terminal: true
}
routes {
@ -13,17 +13,17 @@ logger {
routes {
level: LOG_LEVEL_WARNING
terminal: true
file: true
file: false
}
routes {
level: LOG_LEVEL_ERROR
terminal: true
file: true
file: false
}
routes {
level: LOG_LEVEL_FATAL
terminal: true
file: true
file: false
}
directory: "../log"

16
cmvr-es/config/manager/device_manager.pb.txt
View File

@ -7,14 +7,22 @@ device_manager {
id: "right_hand_cam"
type: DEVICE_TYPE_CAMERA
config_file: "devices/camera/camera.pb.txt"
enable: true
enable: false
}
devices {
id: "cam5"
type: DEVICE_TYPE_CAMERA
config_file: "devices/camera/camera.pb.txt"
enable: true
}
devices {
id: "hand2"
type: DEVICE_TYPE_DEXHAND
config_file: "devices/dexhand/dexhand.pb.txt"
enable: true
enable: false
}
devices {
@ -28,14 +36,14 @@ device_manager {
id: "ti5_motors"
type: DEVICE_TYPE_MOTOR_SYSTEM
config_file: "devices/motor/ti5_motors.pb.txt"
enable: true
enable: false
}
devices {
id: "right_arm"
type: DEVICE_TYPE_ROBOT_ARM
config_file: "devices/arm/arm.pb.txt"
enable: true
enable: false
}
devices {

2
cmvr-es/config/manager/task_manager.pb.txt
View File

@ -5,7 +5,7 @@ task_manager {
run_mode: TASK_RUN_MODE_PERIODIC_STEP
control_period_s: 0.001
config_file: "tasks/touch_screen_task/touch_screen_task.pb.txt"
enable: true
enable: false
}
tasks {
id: "grpc_server"

3
cmvr-es/devices/arm/aubo_arm/include/aubo_arm.h
View File

@ -4,6 +4,7 @@
#include <atomic>
#include <memory>
#include <mutex>
#include <optional>
#include <string>
#include <vector>
@ -46,7 +47,7 @@ public:
Result stopJ(double acceleration) override;
Result moveL(const CartesianPose& target, const MotionOptions& options, FrameType frame = FrameType::Base) override;
Result speedL(const CartesianVelocity& velocity, double acceleration, double duration, FrameType frame = FrameType::Base) override;
Result stopL(double acceleration) override;
Result stopL(std::optional<double> acceleration = std::nullopt) override;
Result stopMotion() override;
Result startServoMode(const ServoOptions& options) override;

2
cmvr-es/devices/arm/aubo_arm/src/aubo_arm.cpp
View File

@ -366,7 +366,7 @@ Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, d
return unsupported_("speedL");
}
Result AuboArm::stopL(double acceleration)
Result AuboArm::stopL(std::optional<double> acceleration)
{
(void)acceleration;
return stopMotion();

3
cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h
View File

@ -4,6 +4,7 @@
#include <atomic>
#include <memory>
#include <mutex>
#include <optional>
#include <string>
#include <unordered_set>
#include <vector>
@ -57,7 +58,7 @@ public:
double acceleration,
double duration,
FrameType frame = FrameType::Base) override;
Result stopL(double acceleration) override;
Result stopL(std::optional<double> acceleration = std::nullopt) override;
Result stopMotion() override;
Result startServoMode(const ServoOptions& options) override;

2
cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp
View File

@ -421,7 +421,7 @@ Result MotorRobotArm::speedL(const CartesianVelocity& velocity,
return cartesian_velocity_controller_->speedL(velocity, acceleration, duration, frame);
}
Result MotorRobotArm::stopL(const double acceleration)
Result MotorRobotArm::stopL(const std::optional<double> acceleration)
{
if (!cartesian_velocity_controller_) {
return Result::success();

3
cmvr-es/devices/arm/robot_arm.h
View File

@ -3,6 +3,7 @@
#include <cstddef>
#include <memory>
#include <optional>
#include <string>
#include <vector>
@ -53,7 +54,7 @@ public:
double acceleration,
double duration,
FrameType frame = FrameType::Base) = 0;
virtual Result stopL(double acceleration) = 0;
virtual Result stopL(std::optional<double> acceleration = std::nullopt) = 0;
virtual Result stopMotion() = 0;
virtual Result moveP(const CartesianPose& target,

1
cmvr-es/devices/motor/abstract_motor.h
View File

@ -27,6 +27,7 @@ namespace cmvr::device{
double limit_q_lb;
double limit_q_ub;
double limit_qd;
double limit_qdd;
};
typedef struct {

26
cmvr-es/devices/motor/motor_system/src/motor_system.cpp
View File

@ -253,9 +253,9 @@ bool MotorSystem::applyConfiguredJointLimits_(const config::MotorGroupConfig& gr
}
} else if (source == config::JOINT_LIMIT_SOURCE_URDF) {
const auto urdf_path =
cmvr::ConfigHelper::resolveResourceFile(group_cfg.joint_limits_urdf_path());
cmvr::ConfigHelper::resolveResourceFile(limits_cfg.urdf_path());
if (urdf_path.empty()) {
CMVR_LOG(ERROR) << "[MotorSystem] joint_limits_urdf_path is empty for motor group: "
CMVR_LOG(ERROR) << "[MotorSystem] joint_limits.urdf_path is empty for motor group: "
<< group_cfg.id();
return false;
}
@ -273,9 +273,9 @@ bool MotorSystem::applyConfiguredJointLimits_(const config::MotorGroupConfig& gr
}
config::JointLimitConfig limit;
limit.set_joint_name(model.names[joint_id]);
limit.set_lower(model.lowerPositionLimit[model.idx_qs[joint_id]]);
limit.set_upper(model.upperPositionLimit[model.idx_qs[joint_id]]);
limit.set_velocity(model.velocityLimit[model.idx_vs[joint_id]]);
limit.set_q_lb(model.lowerPositionLimit[model.idx_qs[joint_id]]);
limit.set_q_ub(model.upperPositionLimit[model.idx_qs[joint_id]]);
limit.set_qd(model.velocityLimit[model.idx_vs[joint_id]]);
limits_by_joint[limit.joint_name()] = limit;
}
} else {
@ -291,15 +291,19 @@ bool MotorSystem::applyConfiguredJointLimits_(const config::MotorGroupConfig& gr
return false;
}
const auto& limit = it->second;
if (!std::isfinite(limit.lower()) || !std::isfinite(limit.upper()) ||
!std::isfinite(limit.velocity()) || limit.upper() <= limit.lower() ||
limit.velocity() <= 0.0) {
if (!std::isfinite(limit.q_lb()) || !std::isfinite(limit.q_ub()) ||
!std::isfinite(limit.qd()) || limit.q_ub() <= limit.q_lb() ||
limit.qd() <= 0.0 || !std::isfinite(limit.qdd()) ||
limit.qdd() < 0.0) {
CMVR_LOG(ERROR) << "[MotorSystem] invalid joint limit for " << motor_cfg.joint_name();
return false;
}
motor_cfg.set_limit_q_lb(limit.lower());
motor_cfg.set_limit_q_ub(limit.upper());
motor_cfg.set_limit_qd(std::abs(limit.velocity()));
motor_cfg.set_limit_q_lb(limit.q_lb());
motor_cfg.set_limit_q_ub(limit.q_ub());
motor_cfg.set_limit_qd(std::abs(limit.qd()));
if (limit.qdd() > 0.0) {
motor_cfg.set_limit_qdd(std::abs(limit.qdd()));
}
}
return true;
}

3
cmvr-es/devices/motor/ti5_motor/ti5_motor.h
View File

@ -21,6 +21,7 @@ namespace cmvr {
info_.limit_q_lb = config.limit_q_lb();
info_.limit_q_ub = config.limit_q_ub();
info_.limit_qd = config.limit_qd() > 0.0 ? config.limit_qd() : 0.5;
info_.limit_qdd = config.limit_qdd() > 0.0 ? config.limit_qdd() : 10.0;
node_id_ = info_.id;
}
@ -48,7 +49,7 @@ namespace cmvr {
// canopen_protocol->seedSdoRequest(node_id_, msgs::CS_WRITE_TWO_BYTES, msgs::CONTROL_WORD_6040, msgs::SUB_INDEX_0, 0x0F,15);
canopen_protocol->setLimitQ(node_id_, info_.limit_q_ub, info_.limit_q_lb);
canopen_protocol->setLimitQd(node_id_, info_.limit_qd);
canopen_protocol->setLimitQdd(node_id_,10.0,-10.0);
canopen_protocol->setLimitQdd(node_id_, info_.limit_qdd, -info_.limit_qdd);
// canopen_protocol->torqueOff(node_id_);
}

85
cmvr-es/service/grpc/src/grpc_arm_service.cpp
View File

@ -2,6 +2,8 @@
#include <google/protobuf/util/time_util.h>
#include "common/base/logging/logger.h"
using google::protobuf::util::TimeUtil;
namespace cmvr::service {
@ -25,6 +27,12 @@ grpc::Status resultToStatus(const device::Result& result)
return grpc::Status(grpc::StatusCode::INTERNAL, result.message);
}
void logRpcSuccess(const char* rpc_name, const std::string& device_id)
{
CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (" << rpc_name
<< "): success, id=" << device_id;
}
device::FrameType toFrameType(const api::ArmFrameType frame)
{
switch (frame) {
@ -130,6 +138,9 @@ grpc::Status gRPCArmServiceImpl::torqueOff(grpc::ServerContext*,
}
const auto result = arm->torqueOff();
fillFeedback(response, result.ok(), result.ok() ? "" : result.message);
if (result.ok()) {
logRpcSuccess("torqueOff", device_id);
}
return resultToStatus(result);
} catch (const std::exception& e) {
fillFeedback(response, false, e.what());
@ -149,6 +160,9 @@ grpc::Status gRPCArmServiceImpl::torqueOn(grpc::ServerContext*,
}
const auto result = arm->torqueOn();
fillFeedback(response, result.ok(), result.ok() ? "" : result.message);
if (result.ok()) {
logRpcSuccess("torqueOn", device_id);
}
return resultToStatus(result);
} catch (const std::exception& e) {
fillFeedback(response, false, e.what());
@ -166,8 +180,13 @@ grpc::Status gRPCArmServiceImpl::moveJ(grpc::ServerContext*,
if (!arm) {
return setDeviceNotFound(response, device_id);
}
return setResponseResult(response, arm->moveJ(toJointPositionCommand(request->target()),
toMotionOptions(request->options())));
const auto result = arm->moveJ(toJointPositionCommand(request->target()),
toMotionOptions(request->options()));
if (result.ok()) {
CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (moveJ): success, id=" << device_id
<< ", positions=" << request->target().position_size();
}
return setResponseResult(response, result);
} catch (const std::exception& e) {
fillFeedback(response->mutable_header(), false, e.what());
return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
@ -184,9 +203,14 @@ grpc::Status gRPCArmServiceImpl::moveL(grpc::ServerContext*,
if (!arm) {
return setDeviceNotFound(response, device_id);
}
return setResponseResult(response, arm->moveL(toCartesianPose(request->target()),
toMotionOptions(request->options()),
toFrameType(request->frame())));
const auto result = arm->moveL(toCartesianPose(request->target()),
toMotionOptions(request->options()),
toFrameType(request->frame()));
if (result.ok()) {
CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (moveL): success, id=" << device_id
<< ", frame=" << request->frame();
}
return setResponseResult(response, result);
} catch (const std::exception& e) {
fillFeedback(response->mutable_header(), false, e.what());
return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
@ -203,9 +227,16 @@ grpc::Status gRPCArmServiceImpl::speedJ(grpc::ServerContext*,
if (!arm) {
return setDeviceNotFound(response, device_id);
}
return setResponseResult(response, arm->speedJ(toJointVelocityCommand(request->velocity()),
request->acceleration(),
request->duration()));
const auto result = arm->speedJ(toJointVelocityCommand(request->velocity()),
request->acceleration(),
request->duration());
if (result.ok()) {
CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (speedJ): success, id=" << device_id
<< ", velocities=" << request->velocity().velocity_size()
<< ", acceleration=" << request->acceleration()
<< ", duration=" << request->duration();
}
return setResponseResult(response, result);
} catch (const std::exception& e) {
fillFeedback(response->mutable_header(), false, e.what());
return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
@ -222,10 +253,17 @@ grpc::Status gRPCArmServiceImpl::speedL(grpc::ServerContext*,
if (!arm) {
return setDeviceNotFound(response, device_id);
}
return setResponseResult(response, arm->speedL(toCartesianVelocity(request->velocity()),
request->acceleration(),
request->duration(),
toFrameType(request->frame())));
const auto result = arm->speedL(toCartesianVelocity(request->velocity()),
request->acceleration(),
request->duration(),
toFrameType(request->frame()));
if (result.ok()) {
CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (speedL): success, id=" << device_id
<< ", acceleration=" << request->acceleration()
<< ", duration=" << request->duration()
<< ", frame=" << request->frame();
}
return setResponseResult(response, result);
} catch (const std::exception& e) {
fillFeedback(response->mutable_header(), false, e.what());
return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
@ -242,7 +280,12 @@ grpc::Status gRPCArmServiceImpl::servoJ(grpc::ServerContext*,
if (!arm) {
return setDeviceNotFound(response, device_id);
}
return setResponseResult(response, arm->servoJ(toJointPositionCommand(request->target())));
const auto result = arm->servoJ(toJointPositionCommand(request->target()));
if (result.ok()) {
CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (servoJ): success, id=" << device_id
<< ", positions=" << request->target().position_size();
}
return setResponseResult(response, result);
} catch (const std::exception& e) {
fillFeedback(response->mutable_header(), false, e.what());
return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
@ -261,6 +304,9 @@ grpc::Status gRPCArmServiceImpl::stopMotion(grpc::ServerContext*,
}
const auto result = arm->stopMotion();
fillFeedback(response, result.ok(), result.ok() ? "" : result.message);
if (result.ok()) {
logRpcSuccess("stopMotion", device_id);
}
return resultToStatus(result);
} catch (const std::exception& e) {
fillFeedback(response, false, e.what());
@ -288,6 +334,9 @@ grpc::Status gRPCArmServiceImpl::getJointState(grpc::ServerContext*,
for (double v : state.velocity) msg->add_velocity(v);
for (double v : state.effort) msg->add_effort(v);
fillFeedback(response->mutable_header(), true);
CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (getJointState): success, id=" << device_id
<< ", joints=" << msg->name_size()
<< ", positions=" << msg->position_size();
return grpc::Status::OK;
} catch (const std::exception& e) {
fillFeedback(response->mutable_header(), false, e.what());
@ -310,6 +359,9 @@ grpc::Status gRPCArmServiceImpl::getPose(grpc::ServerContext*,
: arm->fk(request->base_link(), request->ee_link());
*response->mutable_pose() = toApiCartesianPose(pose);
fillFeedback(response->mutable_header(), true);
CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (getPose): success, id=" << device_id
<< ", pose=(" << pose.x << ", " << pose.y << ", " << pose.z
<< ", " << pose.rx << ", " << pose.ry << ", " << pose.rz << ")";
return grpc::Status::OK;
} catch (const std::exception& e) {
fillFeedback(response->mutable_header(), false, e.what());
@ -327,7 +379,12 @@ grpc::Status gRPCArmServiceImpl::calibrateZeroQ(grpc::ServerContext*,
if (!arm) {
return setDeviceNotFound(response, device_id);
}
return setResponseResult(response, arm->calibrateZeroQ(request->joint_name()));
const auto result = arm->calibrateZeroQ(request->joint_name());
if (result.ok()) {
CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (calibrateZeroQ): success, id=" << device_id
<< ", joint=" << request->joint_name();
}
return setResponseResult(response, result);
} catch (const std::exception& e) {
fillFeedback(response->mutable_header(), false, e.what());
return grpc::Status(grpc::StatusCode::INTERNAL, e.what());

29
cmvr-es/service/grpc/src/grpc_camera_service.cpp
View File

@ -47,6 +47,14 @@ grpc::Status gRPCCameraServiceImpl::GetStatus(grpc::ServerContext* context,
response->mutable_state()->set_fps(state.fps);
response->mutable_state()->set_width(state.width);
response->mutable_state()->set_height(state.height);
CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (GetStatus): success, id=" << dev_id
<< ", initialized=" << state.is_initialized
<< ", opened=" << state.is_opened
<< ", streaming=" << state.is_streaming
<< ", recording=" << state.is_recording
<< ", error=" << state.is_error
<< ", size=" << state.width << "x" << state.height
<< ", fps=" << state.fps;
return grpc::Status::OK;
}
catch(const exception &e) {
@ -72,6 +80,7 @@ grpc::Status gRPCCameraServiceImpl::StartCamera(grpc::ServerContext* context,
}
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (StartCamera): success, id=" << dev_id;
return grpc::Status::OK;
}
catch (const exception &e) {
@ -97,6 +106,7 @@ grpc::Status gRPCCameraServiceImpl::StopCamera(grpc::ServerContext* context,
}
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (StopCamera): success, id=" << dev_id;
return grpc::Status::OK;
}
catch (exception &e) {
@ -150,6 +160,10 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImage(grpc::ServerContext* context,
response->mutable_color_frame()->set_height(image.rows);
response->mutable_color_frame()->set_width(image.cols);
response->mutable_color_frame()->set_codec("none");
CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (GetRGBImage): success, id=" << dev_id
<< ", size=" << image.cols << "x" << image.rows
<< ", cv_type=" << image.type()
<< ", bytes=" << image.total() * image.elemSize();
return grpc::Status::OK;
}
catch (exception &e) {
@ -206,6 +220,10 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImage(grpc::ServerContext* context,
response->mutable_depth_frame()->set_height(image.rows);
response->mutable_depth_frame()->set_width(image.cols);
response->mutable_depth_frame()->set_codec("none");
CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (GetDepthImage): success, id=" << dev_id
<< ", size=" << image.cols << "x" << image.rows
<< ", cv_type=" << image.type()
<< ", bytes=" << image.total() * image.elemSize();
return grpc::Status::OK;
}
catch (exception &e) {
@ -275,6 +293,11 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImages(grpc::ServerContext* context,
response->mutable_depth_frame()->set_height(depth_image.rows);
response->mutable_depth_frame()->set_width(depth_image.cols);
response->mutable_depth_frame()->set_codec("none");
CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (GetRGBDImages): success, id=" << dev_id
<< ", color_size=" << color_image.cols << "x" << color_image.rows
<< ", color_type=" << color_image.type()
<< ", depth_size=" << depth_image.cols << "x" << depth_image.rows
<< ", depth_type=" << depth_image.type();
return grpc::Status::OK;
}
catch (exception &e) {
@ -298,6 +321,8 @@ grpc::Status gRPCCameraServiceImpl::StartRecording(grpc::ServerContext* context,
dev->startRecording(request->video_path());
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (StartRecording): success, id=" << dev_id
<< ", path=" << request->video_path();
return grpc::Status::OK;
}
catch (exception &e) {
@ -321,6 +346,7 @@ grpc::Status gRPCCameraServiceImpl::StopRecording(grpc::ServerContext* context,
dev->stopRecording();
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (StopRecording): success, id=" << dev_id;
return grpc::Status::OK;
}
catch (exception &e) {
@ -351,6 +377,7 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImageStream(grpc::ServerContext* con
}
int nFrameCount = 0;
dev->startStreaming();
CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (GetDepthImageStream): start streaming success, id=" << dev_id;
size_t index = 0;
while (true)
{
@ -420,6 +447,7 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImagesStream(grpc::ServerContext* con
}
int nFrameCount = 0;
dev->startStreaming();
CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (GetRGBDImagesStream): start streaming success, id=" << dev_id;
size_t index = 0;
while (true)
{
@ -494,6 +522,7 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* conte
}
int nFrameCount = 0;
dev->startStreaming();
CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (GetRGBImageStream): start streaming success, id=" << dev_id;
size_t last_sent_index = std::numeric_limits<size_t>::max();
while (true)
{

17
cmvr-es/service/grpc/src/grpc_dexhand_service.cpp
View File

@ -211,6 +211,9 @@ grpc::Status gRPCDexHandServiceImpl::GetStatus(grpc::ServerContext* context,
}
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (GetStatus): success, id=" << dev_id
<< ", initialized=" << state.is_initialized
<< ", dof=" << response->state().hands_size();
return grpc::Status::OK;
}
catch (const std::exception& e) {
@ -246,6 +249,8 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandPos(grpc::ServerContext* context
dev->setPositions(finger_joint_targets);
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (SetDexHandPos): success, id=" << dev_id
<< ", values=" << request->values_size();
return grpc::Status::OK;
}
catch (const std::exception& e) {
@ -285,6 +290,8 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandAngle(grpc::ServerContext* contex
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (SetDexHandAngle): success, id=" << dev_id
<< ", values=" << request->values_size();
return grpc::Status::OK;
}
catch (const std::exception& e) {
@ -320,6 +327,8 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandForce(grpc::ServerContext* contex
dev->setForce(finger_joint_targets);
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (SetDexHandForce): success, id=" << dev_id
<< ", values=" << request->values_size();
return grpc::Status::OK;
}
catch (const std::exception& e) {
@ -355,6 +364,8 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandSpeed(grpc::ServerContext* contex
dev->setVelocities(finger_joint_targets);
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (SetDexHandSpeed): success, id=" << dev_id
<< ", values=" << request->values_size();
return grpc::Status::OK;
}
catch (const std::exception& e) {
@ -386,6 +397,8 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandPresetAct(grpc::ServerContext* co
dev->setPresetAct(presetActId);
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (SetDexHandPresetAct): success, id=" << dev_id
<< ", preset_act_id=" << presetActId;
return grpc::Status::OK;
}
catch (const std::exception& e) {
@ -411,6 +424,8 @@ grpc::Status gRPCDexHandServiceImpl::GetSensorData(grpc::ServerContext* context
appendSensorData(dev->getSensorData(), response);
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (GetSensorData): success, id=" << dev_id
<< ", sensors=" << response->sensor_size();
return grpc::Status::OK;
}
catch (const std::exception& e) {
@ -442,6 +457,7 @@ grpc::Status gRPCDexHandServiceImpl::GetSensorDataStream(grpc::ServerContext* co
return grpc::Status::OK;
}
maybeConfigureRh56FullTactilePolling(dev);
CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (GetSensorDataStream): streaming success, id=" << dev_id;
while (!context->IsCancelled())
{
@ -457,6 +473,7 @@ grpc::Status gRPCDexHandServiceImpl::GetSensorDataStream(grpc::ServerContext* co
std::this_thread::sleep_for(std::chrono::milliseconds(33));
}
CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (GetSensorDataStream): finished, id=" << dev_id;
return grpc::Status::OK;
}
catch (const std::exception& e) {

18
cmvr-es/service/grpc/src/grpc_head_service.cpp
View File

@ -23,6 +23,11 @@ grpc::Status failResponse(ResponseT* response, const std::string& message) {
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
return grpc::Status::OK;
}
void logSuccess(const char* rpc_name, const std::string& device_id) {
CMVR_LOG(DEBUG) << "[gRPCMBioHeadServiceImpl] (" << rpc_name
<< "): success, id=" << device_id;
}
}
gRPCMBioHeadServiceImpl::gRPCMBioHeadServiceImpl()
@ -69,6 +74,7 @@ grpc::Status gRPCMBioHeadServiceImpl::SetExpression(
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
logSuccess("SetExpression", dev_id);
return grpc::Status::OK;
} catch (const std::exception& e) {
response->mutable_header()->set_success(false);
@ -123,6 +129,7 @@ grpc::Status gRPCMBioHeadServiceImpl::StreamExpression(
robot->emergency_stop_requested = false;
first_message = false;
CMVR_LOG(DEBUG) << "[gRPCMBioHeadServiceImpl] (StreamExpression): streaming success, id=" << dev_id;
}
// ✅ 如果紧急停止触发，直接退出
@ -184,6 +191,7 @@ grpc::Status gRPCMBioHeadServiceImpl::StreamExpression(
}
CMVR_LOG(INFO) << "StreamExpression finished for device: " << dev_id;
CMVR_LOG(DEBUG) << "[gRPCMBioHeadServiceImpl] (StreamExpression): finished, id=" << dev_id;
return grpc::Status::OK;
} catch (const std::exception& e) {
CMVR_LOG(ERROR) << "StreamExpression error: " << e.what();
@ -211,6 +219,7 @@ grpc::Status gRPCMBioHeadServiceImpl::GetSystemStatus(
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
logSuccess("GetSystemStatus", dev_id);
return grpc::Status::OK;
}
catch (const exception& e) {
@ -245,6 +254,7 @@ grpc::Status gRPCMBioHeadServiceImpl::EmergencyStop(
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
logSuccess("EmergencyStop", dev_id);
return grpc::Status::OK;
}
catch (const exception& e) {
@ -274,6 +284,7 @@ grpc::Status gRPCMBioHeadServiceImpl::SpeakStart(grpc::ServerContext* context, c
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
logSuccess("SpeakStart", dev_id);
return grpc::Status::OK;
}
catch (const exception& e) {
@ -300,6 +311,7 @@ grpc::Status gRPCMBioHeadServiceImpl::SpeakStop(grpc::ServerContext* context, co
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
logSuccess("SpeakStop", dev_id);
return grpc::Status::OK;
}
catch (const exception& e) {
@ -325,6 +337,7 @@ grpc::Status gRPCMBioHeadServiceImpl::Happy(grpc::ServerContext* context, const
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
logSuccess("Happy", dev_id);
return grpc::Status::OK;
}
catch (const exception& e) {
@ -348,6 +361,7 @@ grpc::Status gRPCMBioHeadServiceImpl::Surprise(grpc::ServerContext* context, con
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
logSuccess("Surprise", dev_id);
return grpc::Status::OK;
}
catch (const exception& e) {
@ -372,6 +386,7 @@ grpc::Status gRPCMBioHeadServiceImpl::ExpressionTired(grpc::ServerContext* conte
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
logSuccess("ExpressionTired", dev_id);
return grpc::Status::OK;
}
catch (const exception& e) {
@ -397,6 +412,7 @@ grpc::Status gRPCMBioHeadServiceImpl::ExpressionAngry(grpc::ServerContext* conte
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
logSuccess("ExpressionAngry", dev_id);
return grpc::Status::OK;
}
catch (const exception& e) {
@ -422,6 +438,7 @@ grpc::Status gRPCMBioHeadServiceImpl::ExpressionSadness(grpc::ServerContext* con
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
logSuccess("ExpressionSadness", dev_id);
return grpc::Status::OK;
}
catch (const exception& e) {
@ -446,6 +463,7 @@ grpc::Status gRPCMBioHeadServiceImpl::ExpressionYawn(grpc::ServerContext* contex
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
logSuccess("ExpressionYawn", dev_id);
return grpc::Status::OK;
}
catch (const exception& e) {

5
cmvr-es/service/grpc/src/grpc_hlc_service.cpp
View File

@ -11,6 +11,7 @@
#include <google/protobuf/util/time_util.h>
#include "common/base/logging/logger.h"
#include "manager/task_manager/include/task_manager.h"
#include "task/touch_screen_task/include/touch_screen_task.h"
@ -74,6 +75,10 @@ grpc::Status gRPCHlcServiceImpl::touch(grpc::ServerContext *context, const cmvr:
}
fillTouchResponse(response, true, "");
CMVR_LOG(DEBUG) << "[gRPCHlcServiceImpl] (touch): success, u=" << request->u()
<< ", v=" << request->v()
<< ", phase=" << cmvr::task::TouchScreenTask::phaseToString(touch_task->phase())
<< ", status=" << cmvr::task::TouchScreenTask::statusToString(touch_task->lastStatus());
return grpc::Status::OK;
} catch (const std::exception& e) {
fillTouchResponse(response, false, e.what());

14
cmvr-es/service/grpc/src/grpc_microphone_service.cpp
View File

@ -40,6 +40,11 @@ grpc::Status gRPCMicroPhoneServiceImpl::GetStatus(grpc::ServerContext* context,
response->mutable_state()->set_error_message(state.error_message);
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
CMVR_LOG(DEBUG) << "[gRPCMicroPhoneServiceImpl] (GetStatus): success, id=" << dev_id
<< ", initialized=" << state.is_initialized
<< ", running=" << state.is_running
<< ", recording=" << state.is_recording
<< ", volume=" << state.volume;
return grpc::Status::OK;
}
catch (const std::exception& e) {
@ -65,6 +70,8 @@ grpc::Status gRPCMicroPhoneServiceImpl::StartRecord(grpc::ServerContext* context
dev->startRecording(request->file_path());
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
CMVR_LOG(DEBUG) << "[gRPCMicroPhoneServiceImpl] (StartRecord): success, id=" << dev_id
<< ", path=" << request->file_path();
return grpc::Status::OK;
}
catch (const std::exception& e) {
@ -87,6 +94,7 @@ grpc::Status gRPCMicroPhoneServiceImpl::StopRecord(grpc::ServerContext* context,
dev->stopRecording();
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
CMVR_LOG(DEBUG) << "[gRPCMicroPhoneServiceImpl] (StopRecord): success, id=" << dev_id;
return grpc::Status::OK;
}
catch (const std::exception& e) {
@ -109,6 +117,7 @@ grpc::Status gRPCMicroPhoneServiceImpl::PauseRecord(grpc::ServerContext* context
dev->pause();
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
CMVR_LOG(DEBUG) << "[gRPCMicroPhoneServiceImpl] (PauseRecord): success, id=" << dev_id;
return grpc::Status::OK;
}
catch (const std::exception& e) {
@ -131,6 +140,7 @@ grpc::Status gRPCMicroPhoneServiceImpl::ResumeRecord(grpc::ServerContext* contex
dev->resume();
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
CMVR_LOG(DEBUG) << "[gRPCMicroPhoneServiceImpl] (ResumeRecord): success, id=" << dev_id;
return grpc::Status::OK;
}
catch (const std::exception& e) {
@ -153,6 +163,8 @@ grpc::Status gRPCMicroPhoneServiceImpl::SetVolume(grpc::ServerContext* context,
dev->setVolume(request->volume());
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
CMVR_LOG(DEBUG) << "[gRPCMicroPhoneServiceImpl] (SetVolume): success, id=" << dev_id
<< ", volume=" << request->volume();
return grpc::Status::OK;
}
catch (const std::exception& e) {
@ -175,6 +187,8 @@ grpc::Status gRPCMicroPhoneServiceImpl::GetVolume(grpc::ServerContext* context,
response->set_volume(dev->getVolume());
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
CMVR_LOG(DEBUG) << "[gRPCMicroPhoneServiceImpl] (GetVolume): success, id=" << dev_id
<< ", volume=" << response->volume();
return grpc::Status::OK;
}
catch (const std::exception& e) {

15
cmvr-es/service/grpc/src/grpc_speaker_service.cpp
View File

@ -40,6 +40,12 @@ grpc::Status gRPCSpeakerServiceImpl::GetStatus(grpc::ServerContext* context,
response->mutable_state()->set_volume(state.volume);
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
CMVR_LOG(DEBUG) << "[gRPCSpeakerServiceImpl] (GetStatus): success, id=" << dev_id
<< ", initialized=" << state.is_initialized
<< ", running=" << state.is_running
<< ", decoding=" << state.is_decoding
<< ", paused=" << state.is_paused
<< ", volume=" << state.volume;
return grpc::Status::OK;
}
catch (const std::exception& e) {
@ -63,6 +69,8 @@ grpc::Status gRPCSpeakerServiceImpl::PlayAudio(grpc::ServerContext* context,
dev->play(request->audio_path());
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
CMVR_LOG(DEBUG) << "[gRPCSpeakerServiceImpl] (PlayAudio): success, id=" << dev_id
<< ", path=" << request->audio_path();
return grpc::Status::OK;
}
catch (const std::exception& e) {
@ -87,6 +95,7 @@ grpc::Status gRPCSpeakerServiceImpl::StopPlayback(grpc::ServerContext* context,
}
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
CMVR_LOG(DEBUG) << "[gRPCSpeakerServiceImpl] (StopPlayback): success, id=" << dev_id;
return grpc::Status::OK;
}
catch (const std::exception& e) {
@ -109,6 +118,7 @@ grpc::Status gRPCSpeakerServiceImpl::PausePlayback(grpc::ServerContext* context,
dev->pause();
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
CMVR_LOG(DEBUG) << "[gRPCSpeakerServiceImpl] (PausePlayback): success, id=" << dev_id;
return grpc::Status::OK;
}
catch (const std::exception& e) {
@ -131,6 +141,7 @@ grpc::Status gRPCSpeakerServiceImpl::ResumePlayback(grpc::ServerContext* context
dev->resume();
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
CMVR_LOG(DEBUG) << "[gRPCSpeakerServiceImpl] (ResumePlayback): success, id=" << dev_id;
return grpc::Status::OK;
}
catch (const std::exception& e) {
@ -153,6 +164,8 @@ grpc::Status gRPCSpeakerServiceImpl::SetVolume(grpc::ServerContext* context,
dev->setVolume(request->volume());
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
CMVR_LOG(DEBUG) << "[gRPCSpeakerServiceImpl] (SetVolume): success, id=" << dev_id
<< ", volume=" << request->volume();
return grpc::Status::OK;
}
catch (const std::exception& e) {
@ -175,6 +188,8 @@ grpc::Status gRPCSpeakerServiceImpl::GetVolume(grpc::ServerContext* context,
response->set_volume(dev->getVolume());
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
CMVR_LOG(DEBUG) << "[gRPCSpeakerServiceImpl] (GetVolume): success, id=" << dev_id
<< ", volume=" << response->volume();
return grpc::Status::OK;
}
catch (const std::exception& e) {

7
cmvr-es/service/grpc/src/grpc_system_service.cpp
View File

@ -4,6 +4,8 @@
#include "../include/grpc_system_service.h"
#include "common/base/logging/logger.h"
using namespace cmvr::device;
using namespace cmvr::device;
using namespace cmvr::service;
@ -18,6 +20,8 @@ grpc::Status gRPCSystemServiceImpl::GetSystemInfo(grpc::ServerContext* context,
response->set_system_name(dmgr_.name());
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
CMVR_LOG(DEBUG) << "[gRPCSystemServiceImpl] (GetSystemInfo): success, name="
<< response->system_name() << ", version=" << response->version();
return grpc::Status::OK;
}
catch (std::exception& e) {
@ -67,6 +71,8 @@ grpc::Status gRPCSystemServiceImpl::GetSystemStatus(grpc::ServerContext* context
}
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
CMVR_LOG(DEBUG) << "[gRPCSystemServiceImpl] (GetSystemStatus): success, devices="
<< response->device_list_size();
return grpc::Status::OK;
}
catch (const std::exception &e) {
@ -92,6 +98,7 @@ grpc::Status gRPCSystemServiceImpl::StopAll(grpc::ServerContext* context,
dmgr_.stop();
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
CMVR_LOG(DEBUG) << "[gRPCSystemServiceImpl] (StopAll): success";
return grpc::Status::OK;
}
catch (std::exception& e) {

8
cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp
View File

@ -506,7 +506,7 @@ void TouchScreenTask::stop() {
void TouchScreenTask::stopUnlocked() {
if (arm_) {
try {
arm_->stopL(0.0);
arm_->stopL();
} catch (...) {
}
}
@ -1337,7 +1337,7 @@ bool TouchScreenTask::stepRetracting() {
}
try {
arm_->stopL(0.0);
arm_->stopL();
} catch (...) {
enterFailed(Status::ROBOT_COMMAND_FAILED);
return false;
@ -1538,7 +1538,7 @@ bool TouchScreenTask::moveToInitPositionIfEnabled() const {
bool TouchScreenTask::handleTouchTriggered(const bool stop_forward_motion) {
if (stop_forward_motion) {
const auto result = arm_->stopL(0.0);
const auto result = arm_->stopL();
if (!result.ok()) {
return false;
}
@ -1675,7 +1675,7 @@ bool TouchScreenTask::startRetractPhase(const Phase next_phase_after_retract,
void TouchScreenTask::enterFailed(const Status status) {
try {
if (arm_) {
arm_->stopL(0.0);
arm_->stopL();
}
} catch (...) {
}

8
protos/cmvr/config/joint_limits_config.proto
View File

@ -9,12 +9,14 @@ enum JointLimitSource {
message JointLimitConfig {
string joint_name = 1;
double lower = 2;
double upper = 3;
double velocity = 4;
double q_lb = 2;
double q_ub = 3;
double qd = 4;
double qdd = 5;
}
message JointLimitsConfig {
JointLimitSource source = 1;
repeated JointLimitConfig joints = 2;
string urdf_path = 3;
}

2
protos/cmvr/config/motor_config/motor_config.proto
View File

@ -9,6 +9,7 @@ message Ti5MotorConfig {
double limit_q_lb = 3;
double limit_q_ub = 4;
double limit_qd = 5;
double limit_qdd = 6;
}
message SocketCanConfig {
@ -40,7 +41,6 @@ message MotorGroupConfig {
repeated Ti5MotorConfig motors = 20;
JointLimitsConfig joint_limits = 30;
string joint_limits_urdf_path = 31;
}
message MotorConfig {