ROS学习笔记(4)——RVIZ中显示双足机器人行走

写在前面

ros版本 kinetic、noetic 亲测可行
结合前面笔记(3)需求：在rviz中实现双足机器人行走。

效果展示

实现方式

1. rviz中机器人运动是通过控制关节运动的角度，来改变机器人在rviz中的显示，所以需要一组关节运动的数据。
2. 通过matlab的双足机器人模型，来模拟生成机器人运动数据，将数据倒入到ros中进行打包发布话题，实现对rviz中机器人运动控制。

解决的问题

1. matlab生成的数据写入excel
2. 读取Excel文件较为繁琐，故采用python进行excel数据读取较为方便
3. python读取Excel数据需要xlrd库，xlrd库的版本问题(本文采用1.2.0版，2.0版本的xlrd库编译时会抛乱七八糟的异常)
4. 接收关节角度并将数据打包发布成rviz可以识别的数据信息，函数即订阅数据，又发布数据(本文通过c++的类定义初始化性质，来解决回调函数中发布消息导致产生未定义消息体的情况，当然也可以用python，将关节数据获取后直接打包处理，发送给rviz可识别的数据)

代码

matlab模拟双足行走数据跳过，直接给出具体数据下载即可

读取Excel数据并发布角度信息话题

#!/usr/bin/env python
# -*- coding:UTF-8 -*-
""" 
    函数功能:
            读取Excel关节运动角度数据，并打包发送关节角度控制话题 
    创建时间:
            2022.02.11
    创建人:
            yqy
"""
import rospy
import xlrd
from std_msgs.msg import Float64MultiArray
import numpy as np

def robot_joint_angle_pub():
    
    rospy.init_node("joint_angle")
    pub = rospy.Publisher("joint_ctrl_angle",Float64MultiArray,queue_size=100)
    rate = rospy.Rate(150)
    buffer = Float64MultiArray()
    buff = np.arange(0,1,0.1)
    left_xl = xlrd.open_workbook(r'/home/yqy/robot/src/robot01_model/excel/left.xlsx')
    right_xl = xlrd.open_workbook(r'/home/yqy/robot/src/robot01_model/excel/right.xlsx')
    left = left_xl.sheet_by_index(0); # sheet索引从0开始
    right = right_xl.sheet_by_index(0); # sheet索引从0开始
    buff_len = left.nrows # 获取数组行数
    for i in range(1,buff_len):
        buff[0] = left.cell_value(i, 3)
        buff[1] = left.cell_value(i, 4)
        buff[2] = left.cell_value(i, 5)
        buff[3] = right.cell_value(i, 3)
        buff[4] = right.cell_value(i, 4)
        buff[5] = right.cell_value(i, 5)
        rospy.loginfo("buff[0] = %.2f buff[1] = %.2f buff[2] = %.2f buff[3] = %.2f buff[4] = %.2f buff[5] = %.2f", buff[0],buff[1],buff[2],buff[3],buff[4],buff[5])
        
        buffer.data = [buff[0],buff[1],buff[2],buff[3],buff[4],buff[5]]
        pub.publish(buffer)
        rate.sleep()
if __name__ == "__main__":
    while not rospy.is_shutdown():
        try:
            robot_joint_angle_pub()
        except rospy.ROSInterruptException:
            pass

关节角度数据的接收及发布关节控制命令

/* 
    函数功能：
            关节角度数据的接收及发布关节控制命令
    创建时间：
            2022.02.11
    创建人： 
            yqy
*/
#include "ros/ros.h"
#include "sensor_msgs/JointState.h"
#include "std_msgs/Float64MultiArray.h"

class joint_state_pub
{
    private:
        ros::NodeHandle nh;
        ros::Publisher pub;
        ros::Subscriber sub;
    public:
        joint_state_pub()
        {
            pub = nh.advertise<sensor_msgs::JointState>("joint_states",1);
            sub = nh.subscribe<std_msgs::Float64MultiArray>("joint_ctrl_angle",1,&joint_state_pub::jointCtrlCollback,this);
        }
        void jointCtrlCollback(const std_msgs::Float64MultiArray::ConstPtr&  buffer);

};
void joint_state_pub::jointCtrlCollback(const std_msgs::Float64MultiArray::ConstPtr&  buffer)
{
    sensor_msgs::JointState joint_state_pub;
    joint_state_pub.header.stamp = ros::Time::now();
    joint_state_pub.header.frame_id = "robot01";
    joint_state_pub.name.resize(6);
    joint_state_pub.position.resize(6);
    joint_state_pub.name[0] = "left_trunk12left_joint1";
    joint_state_pub.position[0] = buffer->data[0];
    joint_state_pub.name[1] = "left_trunk22left_joint2";
    joint_state_pub.position[1] = buffer->data[1];
    joint_state_pub.name[2] = "left_joint32left_foot1";
    joint_state_pub.position[2] = buffer->data[2];
    joint_state_pub.name[3] = "right_trunk12right_joint1";
    joint_state_pub.position[3] = buffer->data[3];
    joint_state_pub.name[4] = "right_trunk22right_joint2";
    joint_state_pub.position[4] = buffer->data[4];
    joint_state_pub.name[5] = "right_joint32right_foot1";
    joint_state_pub.position[5] = buffer->data[5];
    ROS_INFO("buffer is = %.2f  %.2f %.2f  %.2f  %.2f %.2f ",buffer->data[0],buffer->data[1],buffer->data[2],buffer->data[3],buffer->data[4],buffer->data[5]);
    pub.publish(joint_state_pub);
}
int main(int argc, char  *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"state_pub");
    joint_state_pub jsp;
    ros::Rate rate(150);
    rate.sleep(); 
    ros::spin();
    return 0;
}