ROS学习笔记(4)——RVIZ中显示双足机器人行走
写在前面
ros版本 kinetic、noetic 亲测可行
结合前面笔记(3)需求:在rviz中实现双足机器人行走。
效果展示
实现方式
- rviz中机器人运动是通过控制关节运动的角度,来改变机器人在rviz中的显示,所以需要一组关节运动的数据。
- 通过matlab的双足机器人模型,来模拟生成机器人运动数据,将数据倒入到ros中进行打包发布话题,实现对rviz中机器人运动控制。
解决的问题
- matlab生成的数据写入excel
- 读取Excel文件较为繁琐,故采用python进行excel数据读取较为方便
- python读取Excel数据需要xlrd库,xlrd库的版本问题(本文采用1.2.0版,2.0版本的xlrd库编译时会抛乱七八糟的异常)
- 接收关节角度并将数据打包发布成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;
}
版权声明:本文为qq_34994476原创文章,遵循 CC 4.0 BY-SA 版权协议,转载请附上原文出处链接和本声明。