MOCO通用控制器正式支持ROS，SDK下载地址为：

固件需要采用V105以上的固件版本，在完成使用OCU上位机优化步态参数，可以稳定行走后，可基于树莓派采用ROS来控制机器人，目前以传统全向小车底盘的方式来控制四足机器人，具体使用教程如下：

1. SDK的编译

（1）C的SDK

首先从Git下载对应的SDK，并解压到目录文件下：

cd 到build目录下输，入cmake ..之后make编译无误后会在build文件夹下产生对应的moco_sdk.bin文件。

（2）ROS的SDK

从git下载对应的ROS SDK，并解压到对应的catkin工作目录src中，注意该SDK依赖ROS下的串口serial库可能需要提前安装。

之后再catkin_ws根目录下输入catkin_make编译之后输入source ./devel/setup.bash刷新工作空间以找到launch文件

3. 代码运行测试

首先需要使用树莓派串口连接主控制器的usart4端子！！！！

（1）C SDK测试

在build目录下输入./moco_sdk自动补全并执行，之后依据main.c代码中选择的SDK测试模式

完成对机器人速度、姿态的底盘模式参考，具体状态机切换可参考相应程序，另外在moco结构体中封装了底层回传的姿态和运动学数据

（2）ROS的测试

在catkin_ws完成source命令后输入roslaunch moco_ros moco_ros.launch执行，此时本节点需要等待moco/cmd_vel命令发布后才能激活，该话题如果丢失机器人会自动断电保护因此新打开一个控制台可输入如下人工发布速度命令：

rostopic pub -r 1 moco/cmd_vel geometry_msgs/Twist '{linear: {x: 0, y: 0, z: 0}, angular: {x: 0, y: 0, z: 0}}'

此时节点会打印如下信息：

则此时设置对于步态模式即可进行切换，各步态设置对应的参数如下，首先是命令设置：

#define GAIT_IDLE 0
#define GAIT_TROT 1
#define GAIT_FTROT 2
#define GAIT_WALK 3
#define GAIT_ST_RC 4
#define GAIT_ST_IMU 5
#define GAIT_ST_PUSH 6
#define GAIT_RECOVER 7
#define GAIT_FALL 8
#define GAIT_POWER_OFF 99

底层机器人反馈发布的步态模式话题为/moco/gait_now对应定义为

#define M_SAFE 0
#define M_STAND_RC 1
#define M_STAND_IMU 2
#define M_STAND_PUSH 3
#define M_TROT 4
#define M_F_TROT 5
#define M_WALK 6
#define M_RECOVER 7
#define M_FALLING 8
#define M_CLIMB 9
#define M_WALKE 10
#define M_CRAWL 11
#define M_PRONK 12
#define M_BOUND 13

则首先需要进入站立模式，可人工在新控制台输入:

rostopic pub -1 moco/cmd_gait std_msgs/Float32 -- 4  

在机器人站立时可以通过cmd/vel中的angular命令设置姿态角度，如需进入trot则输入:

rostopic pub -1 moco/cmd_gait std_msgs/Float32 -- 1

之后机器人会以cmd/vel linear命令进行底盘速度控制，如需要断点保护输入:

rostopic pub -1 moco/cmd_gait std_msgs/Float32 -- 99

当cmd_vel或ros节点故障杀死下机器人都会马上自动断电。

3. 代码讲解

ROS和C程序采用了相同的SDK代码，对于ROS来说其发布如下数据：

 ros::Publisher pub_odom = n.advertise<nav_msgs::Odometry>("/moco/odom_now", 100); 
 ros::Publisher pub_gait = n.advertise<std_msgs::Float32>("/moco/gait_now",1000);
 ros::Publisher pub_epos_b = n.advertise<std_msgs::Float32MultiArray>("/moco/epos_b_now", 1000);
 ros::Publisher pub_joint_q = n.advertise<std_msgs::Float32MultiArray>("/moco/joint_q_now", 1000);
 ros::Publisher pub_bat = n.advertise<std_msgs::Float32>("bat", 1000);
 ros::Publisher pub_w = n.advertise<std_msgs::Float32>("W", 1000);
 ros::Publisher pub_h = n.advertise<std_msgs::Float32>("H", 1000);
 ros::Publisher pub_l1 = n.advertise<std_msgs::Float32>("L1", 1000);
 ros::Publisher pub_l2 = n.advertise<std_msgs::Float32>("L2", 1000);
 ros::Publisher pub_l3 = n.advertise<std_msgs::Float32>("L3", 1000);

订阅如下数据：

 ros::Subscriber sub_cmd = n.subscribe("/moco/cmd_vel", 1000, callback1);
 ros::Subscriber sub_gait = n.subscribe("/moco/cmd_gait",1000,callback2);
 ros::Subscriber sub_pos_z = n.subscribe("/moco/cmd_pos_z",1000,callback3);

其中需要注意的是关节角度和机体末端位置均是以FR HR FL HL以及大腿 小腿 侧边的数组顺序发布，对于里程计来说部分数据被放置在了机构体covariance元素中，具体订阅请查看代码。

  odom.pose.pose.position.x = moco.pos_n[Xr];
  odom.pose.pose.position.y = moco.pos_n[Yr];
  odom.pose.pose.position.z = moco.pos_n[Zr];
  odom.pose.pose.orientation.x = moco.att[PITr];
  odom.pose.pose.orientation.y = moco.att[ROLr];
  odom.pose.pose.orientation.z = moco.att[YAWr];
  odom.pose.pose.orientation.w = 0;
  odom.pose.covariance.elems[0]= moco.acc_b[Xr];
  odom.pose.covariance.elems[1]= moco.acc_b[Yr];
  odom.pose.covariance.elems[2]= moco.acc_b[Zr];
  //set the velocity
  odom.child_frame_id = "base_link";
  odom.twist.twist.linear.x = moco.spd_b[Xr];
  odom.twist.twist.linear.y = moco.spd_b[Yr];
  odom.twist.twist.linear.z = moco.spd_b[Zr];
  odom.twist.twist.angular.x = moco.att_rate[PITr];
  odom.twist.twist.angular.y = moco.att_rate[ROLr];
  odom.twist.twist.angular.z = moco.att_rate[YAWr];
  odom.twist.covariance.elems[0]= moco.td_state[0];
  odom.twist.covariance.elems[1]= moco.td_state[1];
  odom.twist.covariance.elems[2]= moco.td_state[2];
  odom.twist.covariance.elems[3]= moco.td_state[3];

可以使用如:

rostopic echo /moco/odom_now/pose/pose/orieation