在不利用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>