ROS学习笔记(3)——通过脚本控制rviz中创建的机器人运动

写在前面

需求：rviz中创建的机器人模型，通过运行脚本方式控制其运动。

ros版本 kinetic 、noetic 两个版本亲测都可以。

整体效果

思路

机器人描述文件xacro，通过joint(关节)来控制机器人连杆与连杆之间的运动，所以可以通过控制关节，发布关节相关话题，即可以实现机器人在rviz中运动。

实现

上述整体效果中机器人模型文件可见：xacro文件

#include "ros/ros.h"
#include "sensor_msgs/JointState.h"
// #include "std_msgs/Header.h"
int main(int argc, char  *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"state_pub");
    ros::NodeHandle nh;
    ros::Publisher pub = nh.advertise<sensor_msgs::JointState>("joint_states",1000);
    sensor_msgs::JointState joint_state_pub;
    ros::Rate rate(100);
    while (ros::ok())
    {
        
        for (double i = -314; i <= 314 ; i++)
        {   
            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);
            //不知道为什么这种方式不可以
		    // jonit_state_pub.name = {    "left_trunk12left_joint1", 
		    //                             "left_trunk22left_joint2", 
		    //                             "left_joint32left_foot1", 
		    //                             "right_trunk12right_joint1",
		    //                             "right_trunk22right_joint2",
		    //                             "right_joint32right_foot1"      };
		 
            joint_state_pub.name[0] = "left_trunk12left_joint1";// name是xacro文件中定义的关节(joint)
            joint_state_pub.position[0] = ( i / 100 );	//控制运动
            joint_state_pub.name[1] = "left_trunk22left_joint2";
            joint_state_pub.position[1] = ( i /100 );
            joint_state_pub.name[2] = "left_joint32left_foot1";
            joint_state_pub.position[2] = 0;
            joint_state_pub.name[3] = "right_trunk12right_joint1";
            joint_state_pub.position[3] = 0;
            joint_state_pub.name[4] = "right_trunk22right_joint2";
            joint_state_pub.position[4] = 0;
            joint_state_pub.name[5] = "right_joint32right_foot1";
            joint_state_pub.position[5] = 0;

            pub.publish(joint_state_pub);
            ROS_INFO(" i = %.2f",i/100);
            rate.sleep(); 
        }
        ros::spinOnce();   
    }
    return 0;
}

注意

这里不能之修改某一个或某几个joint_state_pub.position[x]，每次发布后需要重新填充msg文件中的每一个变量。

扩展

实物中，通过串口传输数据关节数据，ros截获串口数据进行打包，并发送关节数据，方针运动关节通过订阅串口发送的关节数据，可以实现实物与rviz同步运动。