利用joint_states发布消息，去控制机械臂——ROS机械臂学习笔记(二)

在不利用moveit的情况下，实现机械臂的运动，目前想到的就是向话题joint_states添加有关关节的消息，或者在终端发布话题消息，当然也可以使用自带的joint_states_publisher的GUI插件。但是经过这几天的一些测试，发现这三种方法都会出问题，比如你运行了节点或者移动GUI 上面的滑块，结果发现rviz中的模型不运动，这个问题一直很纠结，不知道是不是rviz的问题。

终端上发布的一个话题消息

 rostopic pub -1 /joint_states sensor_msgs/JointState '{header: auto, name: ['joint1', 'joint2', 'joint3'], position: [1, 0.5418, -1], velocity: [], effort: []}'

下面是相应的代码。

#include<iostream>
#include <string>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <robot_state_publisher/robot_state_publisher.h>
using namespace std;

int main(int argc, char** argv)
 {
    ros::init(argc, argv, "state_1_publisher"); //节点的名称
    ros::NodeHandle n;
    ros::Publisher joint_pub = n.advertise<sensor_msgs::JointState>("joint_states", 1);    //设置一个发布者，将消息发布出去,发布到相应的节点中去
    ros::Rate loop_rate(10);    //这个设置的太大，效果很不好，目前觉得为10最好了
    const double degree = M_PI/180;
    const double radius = 2;
    int i=-69;

    // robot state
    double angle= 0;
    // message declarations
    sensor_msgs::JointState joint_state;
  //  for(int j=0;j<6;++j)
   // {
        //joint_state.velocity.push_back(1);
        //joint_state.effort.push_back(200);
    //}

   

    while (ros::ok()) {
        //update joint_state
        joint_state.header.stamp = ros::Time::now();
        joint_state.name.resize(6);
        joint_state.position.resize(6);
	joint_state.name[0]="joint1";
        joint_state.position[0] = i*degree;
	joint_state.name[1] ="joint2";
        joint_state.position[1] = i*degree;
	joint_state.name[2] ="joint3";
        joint_state.position[2] = i*degree;
        joint_state.name[3] ="joint4";
        joint_state.position[3] = i*degree;
	joint_state.name[4] ="joint5";
        joint_state.position[4] = i*degree;
	joint_state.name[5] ="joint6";
        joint_state.position[5] = i*degree;
        if(i<=70)
            i++;
        else
            i=-69; 

        //send the joint state and transform
        joint_pub.publish(joint_state);

        // This will adjust as needed per iteration
        loop_rate.sleep();
    }

    return 0;
}

一般在初学机械臂的过程在应用launch时，都会加载节点joint_state_publisher 去发布消息，一开始我以为这个只是针对GUI来发布关节的状态消息，但是在后来发现即使把参数设置为 False，该节点依然还是会发布消息。
因此用户在自定义的节点中利用joint_states 的话题发布消息的时候，在launch文件中不应该再次加载节点joint_state_publisher，需要删除这个节点，否则你会发现rviz中的机械臂在运动的过程中会经常性的回到原位，在原位和自定义节点中发布的关节位置之间来回移动。不过删除这个 joint_state_publisher 节点之后，在一开始加载的时候会发现RVIZ中的模型不对，但是没关系，你重新加载一个发布joint_states 话题的节点，RVIZ中的机械臂就会恢复正常，然后你可以开心的去控制你的机械臂运动了。

<node 
    name="joint_state_publisher" 
    pkg="joint_state_publisher" 
    type="joint_state_publisher">
    <param name="use_gui" value="False"/>
  </node>