一、简介
- 环境版本:
- 虚拟机系统:Ubuntu 18.04.6 LTS
- 虚拟机版本:VMware® Workstation 17 Pro
- ROS版本:Melodic
- 本文档基于遨博官方ros功能包开发,在aubo_driver下自定义一个订阅者一个发布者节点实现对遨博机械臂的控制。主要调用了 "aubo_driver/serviceinterface.h"中提供的接口函数
二、调试过程
1、自定义话题msg
在 ROS 通信协议中,数据载体是一个较为重要组成部分,ROS 中通过 std_msgs 封装了一些原生的数据类型,比如:String、Int32、Int64、Char、Bool、Empty… 但是,这些数据一般只包含一个 data 字段,结构的单一意味着功能上的局限性,当传输一些复杂的数据,比如: 激光雷达的信息… std_msgs 由于描述性较差而显得力不从心,这种场景下可以使用自定义的消息类型;msgs只是简单的文本文件,每行具有字段类型和字段名称,可以使用的字段类型有:
int8, int16, int32, int64 (或者无符号类型: uint*)
float32, float64
string
time, duration
other msg files
variable-length array[] and fixed-length array[C]
ROS中还有一种特殊类型:Header,标头包含时间戳和ROS中常用的坐标帧信息。会经常看到msg文件的第一行具有Header标头。
- 定义msg文件
在遨博ros功能包的aubo_driver下新建msg目录,添加point.msg文件
#src/aubo_robot/aubo_driver/msg/point.msg
float32 x //x轴坐标
float32 y //y轴坐标
float32 z //z轴坐标
float32 floag //解析指令标志
- 编辑配置文件
在package.xml中添加编译依赖于与执行依赖
<build_depend>message_generation</build_depend>
<run_depend>message_runtime</run_depend>
在CMakeLists.txt编辑msg相关配置
find_package(catkin REQUIRED COMPONENTS
sensor_msgs
roscpp
std_msgs
aubo_msgs
message_generation
)
## 配置 msg 源文件
add_message_files(
FILES
Point.msg
)
# 生成消息时依赖于 std_msgs
generate_messages(
DEPENDENCIES
std_msgs
)
catkin_package(
INCLUDE_DIRS include
CATKIN_DEPENDS sensor_msgs roscpp std_msgs aubo_msgs message_runtime
)
- 编译后查看调用msg的中间文件:
C++ 需要调用的中间文件(…/工作空间/devel/include/包名/xxx.h)
Python 需要调用的中间文件(…/工作空间/devel/lib/python3/dist-packages/包名/msg)
- 编写订阅着节点
#include "aubo_driver/serviceinterface.h"
#include <ros/ros.h>
#include <iostream>
#include "../../../../devel/include/aubo_driver/Point.h"
// #include <window.h>
ServiceInterface robotService;
//机械臂上电和初始化
void main_manipulator(const aubo_driver::Point::ConstPtr& msg)
{
int ret = aubo_robot_namespace::InterfaceCallSuccCode;
/** 接口调用: 登录 ***/
std::string s = "192.168.2.137";
ret = robotService.robotServiceLogin(s.c_str(), 8899, "aubo", "123456");
if(ret == aubo_robot_namespace::InterfaceCallSuccCode)
{
std::cout << "登录成功" << std::endl;
}
else
{
std::cerr << "登录失败" << std::endl;
}
/** 如果是连接真实机械臂,需要对机械臂进行初始化 **/
aubo_robot_namespace::ROBOT_SERVICE_STATE result;
//工具动力学参数
aubo_robot_namespace::ToolDynamicsParam toolDynamicsParam;
memset(&toolDynamicsParam, 0, sizeof(toolDynamicsParam));
ret = robotService.rootServiceRobotStartup(toolDynamicsParam/**工具动力学
参数**/,
6 /*碰撞等级*/,
true /*是否允许读取位
姿 默认为 true*/,
true, /*保留默认为 true
*/
1000, /*保留默认为 100
0 */
result); /*机械臂初始化*/
if(ret == aubo_robot_namespace::InterfaceCallSuccCode)
{
std::cerr << "机械臂初始化成功." << std::endl;
}
else
{
std::cerr << "机械臂初始化失败." << std::endl;
}
//初始化运动属性
robotService.robotServiceInitGlobalMoveProfile();
//设置关节运动最大加速度
aubo_robot_namespace::JointVelcAccParam jointMaxAcc;
jointMaxAcc.jointPara[0] = 6*M_PI/180;
jointMaxAcc.jointPara[1] = 6*M_PI/180;
jointMaxAcc.jointPara[2] = 6*M_PI/180;
jointMaxAcc.jointPara[3] = 6*M_PI/180;
jointMaxAcc.jointPara[4] = 6*M_PI/180;
jointMaxAcc.jointPara[5] = 6*M_PI/180;
robotService.robotServiceSetGlobalMoveJointMaxAcc(jointMaxAcc);
//设置关节运动最大速度
aubo_robot_namespace::JointVelcAccParam jointMaxVelc;
jointMaxVelc.jointPara[0] = 6*M_PI/180;
jointMaxVelc.jointPara[1] = 6*M_PI/180;
jointMaxVelc.jointPara[2] = 6*M_PI/180;
jointMaxVelc.jointPara[3] = 6*M_PI/180;
jointMaxVelc.jointPara[4] = 6*M_PI/180;
jointMaxVelc.jointPara[5] = 6*M_PI/180;
robotService.robotServiceSetGlobalMoveJointMaxVelc(jointMaxVelc);
// //起始路点
// double initAngle[6] = {};
// initAngle[0] = 173.108713*M_PI/180;
// initAngle[1] = -12.075005*M_PI/180;
// initAngle[2] = -83.663342*M_PI/180;
// initAngle[3] = -15.641249*M_PI/180;
// initAngle[4] = -89.140000*M_PI/180;
// initAngle[5] = -28.328713*M_PI/180;
// //关节运动到起始路点
// robotService.robotServiceJointMove(initAngle, true);
}
//机械臂的移动
void movelToTargetPosition1(const aubo_driver::Point::ConstPtr& msg)
{
//设置工具参数,为法兰盘中心
aubo_robot_namespace::ToolInEndDesc toolEnd;
toolEnd.toolInEndPosition.x = 0;
toolEnd.toolInEndPosition.y = 0;
toolEnd.toolInEndPosition.z = 0;
toolEnd.toolInEndOrientation.w = 1;
toolEnd.toolInEndOrientation.x = 0;
toolEnd.toolInEndOrientation.y = 0;
toolEnd.toolInEndOrientation.z = 0;
// ServiceInterface robotService;
//设置基坐标系
aubo_robot_namespace::CoordCalibrateByJointAngleAndTool baseCoord;
baseCoord.coordType = aubo_robot_namespace::BaseCoordinate;
//设置目标位置,法兰盘中心在基坐标系下的位置
aubo_robot_namespace::Pos posOnBase;
posOnBase.x = msg->x;
posOnBase.y = msg->y;
posOnBase.z = msg->z;
std::cout<<msg->x<<std::endl;
//直线运动至目标位置
robotService.robotMoveLineToTargetPosition(baseCoord, posOnBase, toolEnd, true);
// sleep(300);
}
void move_stop(const aubo_driver::Point::ConstPtr& msg)
{
int a = robotService.robotMoveFastStop(); //运动停止
}
//机械臂下电
void move_shutdown(const aubo_driver::Point::ConstPtr& msg)
{
/** 机械臂 Shutdown **/
robotService.robotServiceRobotShutdown();
/** 接口调用: 退出登录**/
robotService.robotServiceLogout();
std::cout<<"机械臂断电成功"<<std::endl;
}
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
ros::init(argc,argv,"move_line_to");
ros::NodeHandle nh;
// 创建一个Subscriber,订阅名为chatter的topic,注册回调函数chatterCallback
ros::Subscriber manipulator = nh.subscribe("main_manipulator", 0, main_manipulator);
ros::Subscriber line_to = nh.subscribe<aubo_driver::Point>("move_line_to", 1, movelToTargetPosition1);
ros::Subscriber stop = nh.subscribe("move_stop", 2, move_stop);
ros::Subscriber shutdown = nh.subscribe("move_shutdown", 3, move_shutdown);
// main_manipulator();
/* code */
ros::spin();
return 0;
}
- 编写发布者节点
// #include "std_msgs/String.h"
#include <ros/ros.h>
#include <iostream>
#include <netinet/in.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include<sstream>
#include<string>
#include<vector>
#include <unistd.h>
#include "../../../devel/include/aubo_driver/Point.h"
#include <thread>
#define HELLO_WORLD_SERVER_PORT 8888
#define LENGTH_OF_LISTEN_QUEUE 20
#define BUFFER_SIZE 1024
using namespace aubo_driver;
std::vector<std::string> splitString(const std::string& input, char delimiter)
{
std::vector<std::string> result;
std::stringstream ss(input);
std::string token;
while (std::getline(ss, token, delimiter)) {
result.push_back(token);
}
return result;
}
int main(int argc, char **argv)
{
setlocale(LC_ALL,"");
ros::init(argc,argv,"controller");
ros::NodeHandle n;
ros::Publisher chatter_manipulator = n.advertise<aubo_driver::Point>("main_manipulator",0);
ros::Publisher chatter_pub = n.advertise<aubo_driver::Point>("move_line_to",1); //机械臂上电和移动
ros::Publisher chatter_stop = n.advertise<aubo_driver::Point>("move_stop",2); //机械臂停止
ros::Publisher chatter_shutdown = n.advertise<aubo_driver::Point>("move_shutdown",3); //机械臂退出
int sockfd = socket(AF_INET, SOCK_STREAM, 0);
if (sockfd == -1)
{
ROS_ERROR("Failed to create socket.");
return 0;
}
struct sockaddr_in serverAddress{};
serverAddress.sin_family = AF_INET;
serverAddress.sin_port = htons(8888); // 指定服务器监听的端口号
serverAddress.sin_addr.s_addr = inet_addr("192.168.2.139");
socklen_t addr_len = sizeof(serverAddress);
// ROS_INFO("Receive data len: %d",addr_len);
if (connect(sockfd,(struct sockaddr *)(&serverAddress), addr_len) < 0)
{
ROS_ERROR("Connection failed.");
}
while (1)
{
char buffer[sizeof(aubo_driver::Point) * 4];
aubo_driver::Point pt;
memset(buffer, 0, sizeof(buffer));
ssize_t bytesReceived = recv(sockfd, buffer, sizeof(buffer), 0);
if (bytesReceived < 0 || bytesReceived == 0)
{
ROS_ERROR("Failed to receive coordinates.");
return 0;
}
std::vector<std::string> result = splitString(buffer, ',');
pt.x = atof(result[0].c_str());
pt.y = atof(result[1].c_str());
pt.z = atof(result[2].c_str());
pt.floag = atof(result[3].c_str());
// pt.floag = flag;
// pt.x = 0.049;
// pt.y = -0.251;
// pt.z = 0.454;
// pt.floag = 0;
ROS_INFO("receive x: [%.3f],y:[%.3f],z:[%.3f],floag:[%.2f]", pt.x,pt.y,pt.z,pt.floag);
if(pt.floag == 0) //事件判断 0、上电初始化
{
chatter_manipulator.publish(pt);
}
else if(pt.floag == 1) //事件判断 1、移动
{
chatter_pub.publish(pt);
}
else if (pt.floag == 2) //2、停止
{
chatter_stop.publish(pt);
}
else if (pt.floag == 3) //3、退出
{
chatter_shutdown.publish(pt);
}
// close(client_socket);
// close(server_socket);
}
}
- 编辑CMakelist.txt
set(${PROJECT_NAME}_SOURCES4
src/user_driver.cpp
)
set(${PROJECT_NAME}_SOURCES5
src/user_publisher.cpp
)
add_executable(user_driver ${${PROJECT_NAME}_SOURCES4})
add_executable(user_publisher ${${PROJECT_NAME}_SOURCES5})
target_link_libraries(user_driver ${catkin_LIBRARIES} auborobotcontroller otgLib)
target_link_libraries(user_publisher ${catkin_LIBRARIES} auborobotcontroller otgLib)
- 编译后运行节点测试
roscore
rosrun aubo_driver user_driver
rosrun aubo_driver user_publisher
使用rqt_graph查看ROS计算图
- 编写launch文件
<launch>
<!-- 运行订阅者驱动 -->
<node pkg="aubo_driver" type = "user_driver" name = "driver" output = "screen" />
<!-- 运行发布者驱动,10s发布一次-->
<node pkg="aubo_driver" type = "user_publisher" name = "controler" output = "screen" />
</launch>
运行结果如下:
登录成功
[ INFO] [1705569517.792532709]: x: [0.05],y:[-0.25],z:[0.45],floag:[0.00]
机械臂初始化成功.
0.049
[ INFO] [1705569527.792883846]: x: [0.05],y:[-0.25],z:[0.45],floag:[0.00]
机械臂断电成功