8.18 传统方法控制接口
请确保本体驱动和传统运控都已经启动。手柄向右拨动H键,可通过下述ROS service切换状态,通过topic下发速度指令,遥控器按键和摇杆无效(仅C键‘僵停’有效),H置中恢复遥控器功能。
8.18.1 运动模式切换
-
接口名称:
/hric/motion/set_motion_mode -
类型:
service -
描述: 请求切换运动模式,适用于天工行者全系列。
-
发布模块: 运动控制
-
client模块: 机器人管理
-
消息类型:
hric_msgs/SetMotionMode.srvService msg 参数名 参数类型 是否必填 备注 请求字段 walk_mode_request uint8 是 # 请求切换的运动模式, 0: start1: stop2: zero3: stand4: walkis_need_swing_arm bool 是 walk是否需要摆臂,true:需要,false:不需要(由于操作版本,locomotion不控手臂,因此设置true或false都可以) 回复字段 sucess bool 是 表示服务调用是否成功 error_code uint32 是 错误码NONE=0 UNKNOWN=400 -
示例命令
cd ros2ws
source install/setup.bash
# 僵停:
ros2 service call /hric/motion/set_motion_mode hric_msgs/srv/SetMotionMode "{walk_mode_request: 1, is_need_swing_arm: false}"
# 回零:
ros2 service call /hric/motion/set_motion_mode hric_msgs/srv/SetMotionMode "{walk_mode_request: 2, is_need_swing_arm: false}"
# 站立:
ros2 service call /hric/motion/set_motion_mode hric_msgs/srv/SetMotionMode "{walk_mode_request: 3, is_need_swing_arm: false}"
# 原地踏步(回零状态放到地面后,直接调用本接口的话可以从回零状态进入原地踏步状态,但是比较危险,不要这样操作,先进入站立状态,再进入原地踏步):
ros2 service call /hric/motion/set_motion_mode hric_msgs/srv/SetMotionMode "{walk_mode_request: 4, is_need_swing_arm: false}"
# 原地跑步:
ros2 service call /hric/motion/set_motion_mode hric_msgs/srv/SetMotionMode "{walk_mode_request: 5, is_need_swing_arm: false}"
8.18.2 矢量速度控制接口
-
接口名称:
/hric/robot/cmd_vel -
类型:
topic -
描述: 发布速度指令,控制机器人运动,适用天工行者全系列。
-
发布模块: 遥控器
-
订阅模块: 机器人管理
-
消息类型:
geometry_msgs/TwistStamped.msggeometry_msgs/msg/TwistStamped 结构如下:
std_msgs/Header header
geometry_msgs/Twist twist
其中 geometry_msgs/Twist 结构又如下:
geometry_msgs/Vector3 linear
geometry_msgs/Vector3 angular
linear 表示线速度 (单位 m/s),x 表示前后移动,y 表示左右平移。
angular 表示角速度 (单位 rad/s),z 表示绕 z 轴转动(左转、右转)。 -
消息频率:20hz
-
发布示例:
cd ros2ws
source install/setup.bash
# x方向速度为0.15,也就是速度0.15前进
ros2 topic pub /hric/robot/cmd_vel geometry_msgs/msg/TwistStamped "{header: {frame_id: 'base_link' }, twist: {linear: {x: 0.15, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}}"
# x方向速度为0.20,也就是速度0.20前进
ros2 topic pub /hric/robot/cmd_vel geometry_msgs/msg/TwistStamped "{header: {frame_id: 'base_link' }, twist: {linear: {x: 0.20, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}}"
# 所有线速度角速度均为0.0,也就是原地踏步
ros2 topic pub /hric/robot/cmd_vel geometry_msgs/msg/TwistStamped "{header: {frame_id: 'base_link' }, twist: {linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}}"
# 以0.1的角速度左转
ros2 topic pub /hric/robot/cmd_vel geometry_msgs/msg/TwistStamped "{header: {frame_id: 'base_link' }, twist: {linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.1}}}"
# 以0.1的角速度右转
ros2 topic pub /hric/robot/cmd_vel geometry_msgs/msg/TwistStamped "{header: {frame_id: 'base_link' }, twist: {linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: -0.1}}}"
# 最后为了让机器人再进入原地踏步状态需要再调用一次接口,线速度角速度均传0.0
8.18.3 机器人下肢运动状态发布
-
接口名称:
/hric/motion/status -
类型:
topic -
描述: 发布运动控制模块状态和速度
-
发布模块: 运动控制
-
发布频率: 20hz
-
订阅模块: 机器人管理、定位、导航
-
消息类型:
hric_msgs/MotionStatus.msg参数名 参数类型 是否必填 备注 header Header 是 时间戳和坐标系 velocity geometry_msgs/Twist.msg 是 机器人腰部坐标系速度 walk_mode uint8 是 行走状态, 0:stop,1:zero,2:zero_2_stand,3:stand,4:stand_2_walk,5:walk,6:startis_console_control bool 是 是否为遥控器操控,true:遥控器操纵 false:导航操控 is_swing_arm bool 是 是否为walk摆臂状态。 error_code uint32 是 错误码NONE=0 UNKNOWN=400 -
消息格式示例:
{
"header": {
"seq": 1,
"stamp": {
"secs": 1622548800,
"nsecs": 0
},
"frame_id": ""
},
"velocity": {
"linear": {
"x": 1.0,
"y": 0.0,
"z": 0.0
},
"angular": {
"x": 0.0,
"y": 0.0,
"z": 0.0
}
},
"walk_mode": 6,
"error_code": 0
}