利用网络和Joint States话题消息,实现仿真机械臂和实际的机械臂实现同步运动。
下位机:把和实际机械臂相连的电脑称为下位机。
上位机:装有ubuntu的电脑称为上位机。
在一篇的博客中利用joint_states发布消息,去控制机械臂, 我们利用joint states话题去发布关节状态的消息,然后rviz中的机械臂接受到相应的消息之后就会运动。现在我们在下位机中用Socket写一个通信程序,把读取到的实际机械臂的关节角度消息发送给上位机中,然后在上位机中写一个节点,用来接受消息,并且把消息填充到joint states中,最后发布出去。这样就可以实现rviz中的机械臂和实际的机械臂实现同步运动了。
上位机中的节点程序如下:
// receive the news of real joint angle form the TCP/IP,and use the topic JointState to publishe the news.
#include <unistd.h>
#include <stdio.h>
#include<iostream>
#include <sys/socket.h>
#include <netinet/in.h>
#include <sys/types.h>
#include <stdlib.h>
#include <string.h>
#include <arpa/inet.h>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#define SERVER_PORT 17999
#define LENGTH_OF_LISTEN_QUEUE 20
#define BUFFER_SIZE 10
double b=1;
int main(int argc, char* argv[]) // (int argc, char* argv[])
{
ros::init(argc, argv, "send_denso_date");
ros::NodeHandle n;
ros::Rate loop_rate(1000); //下位机中的发送频率是1000HZ ,不相同的话,不能实现同步。
ros::Publisher joint_pub = n.advertise<sensor_msgs::JointState>("joint_states", 1000); //设置一个发布者,将消息发布出去,发布到相应的节点中去
sensor_msgs::JointState joint_state;
//网络设置部分
struct sockaddr_in server_addr;
int server_socket;
int opt = 1;
double a=90;
bzero(&server_addr, sizeof(server_addr)); // 置字节字符串前n个字节为0,包括'\0'
server_addr.sin_family = AF_INET;
server_addr.sin_addr.s_addr = htons(INADDR_ANY); // 转小端,INADDR_ANY就是指定地址为0.0.0.0的地址
server_addr.sin_port = htons(SERVER_PORT);
// 创建一个Socket
server_socket = socket(PF_INET, SOCK_STREAM, 0);
if (server_socket < 0)
{
printf("Create Socket Failed!\n");
exit(1);
}
// bind a socket
setsockopt(server_socket, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt));
if(bind(server_socket, (struct sockaddr*)&server_addr, sizeof(server_addr)))
{
printf("Server Bind Port: %d Failed!\n", SERVER_PORT);
exit(1);
}
// 监听Socket
if (listen(server_socket, LENGTH_OF_LISTEN_QUEUE))
{
printf("Server Listen Failed!\n");
exit(1);
}
while(1)
{
struct sockaddr_in client_addr;
int client_socket;
socklen_t length;
double Buffer[6];
// 连接客户端Socket
length = sizeof(client_addr);
client_socket = accept(server_socket, (struct sockaddr*)&client_addr, &length);
if (client_socket < 0)
{
printf("Server Accept Failed!\n");
break;
}
// 从客户端接收数据
while(1)
{
bzero(Buffer, BUFFER_SIZE);
length = recv(client_socket, (char*)&Buffer, sizeof(Buffer), 0); //接受数据
joint_state.header.stamp = ros::Time::now(); //创建相应的消息
joint_state.name.resize(6);
joint_state.position.resize(6);
//joint_state.name={"joint1","joint2""joint3""joint4""joint5""joint6"}; //不知道这个为什么不可以。
joint_state.name[0]="joint1";
joint_state.position[0] = Buffer[0];
joint_state.name[1] ="joint2";
joint_state.position[1] = Buffer[1];
joint_state.name[2] ="joint3";
joint_state.position[2] = Buffer[2];
joint_state.name[3] ="joint4";
joint_state.position[3] = Buffer[3];
joint_state.name[4] ="joint5";
joint_state.position[4] = Buffer[4];
joint_state.name[5] ="joint6";
joint_state.position[5] =Buffer[5];
joint_pub.publish(joint_state);
loop_rate.sleep();
send(client_socket, (char*)&b, sizeof(b), 0);
if (length < 0)
{
printf("Server Recieve Data Failed!\n");
break;
}
if ('q' == Buffer[0])
{
printf("Quit!\n");
break;
}
//printf("%s\n", Buffer);
std::cout<<Buffer[0]<<std::endl; //读取数据,只有一个数据
break;
}
close(client_socket);
}
close(server_socket);
return 0;
}
在这种方法中,所有的控制和路径点都是在下位机中实现的和给定的。有一些需要注意的地方就是两者的发送和接受的频率要一致,如果不一致的话,rviz中的模型就会有一种幻影,不能实现同步。
还有一种方法是把moveit中运动规划的轨迹点下发给下位机,下位机接受到相应的轨迹点后进行运动。剩下的依然是在下位机中用Socket写一个通信程序,把读取到的实际机械臂的关节角度消息发送给上位机中,然后在上位机中写一个节点,用来接受消息,并且把消息填充到joint states中,最后发布出去。这样就可以实现rviz中的机械臂和实际的机械臂实现同步运动了。**
在第二种方法中,控制是在下位机中实现的,但是路径点是在上位机中给定的。这种方法目前还在尝试中。