环境:Ubuntu16.04+ROS Kinetic
ROS与底层串口通讯有两种方式:
- 底层作为ROS节点:https://blog.csdn.net/Kalenee/article/details/80644896
- 采用传统的串口通讯方式进行通讯:https://blog.csdn.net/Kalenee/article/details/82422196
硬件:
Arduino mega2560(需拥有两个串口)
总线舵机(可采用dynamixel舵机,其带有ROS的功能包dynamixel_controllersTutorials)
前期准备:
- 完成MoveIt!配置
- 完成机器人在ROS平台的搭建
一、配置rosserial_arduino
1. 安装环境
sudo apt-get install ros-kinetic-rosserial-arduino
sudo apt-get install ros-kinetic-rosserial
2. 配置ros_lib
sketchbook为Arduino功能包位置
cd <sketchbook>/libraries
rm -rf ros_lib
rosrun rosserial_arduino make_libraries.py .
3. 检查环境
提示:Window 环境的话,可以在Linux环境下配置好环境后将ros_lib文件夹提取到 Window 环境下并导入到Arduino
二、程序实现
- 发布器初始化
ros::Publisher _pub("/topic", &_msg);
nh.advertise(_pub);
- 订阅器初始化
void messageCb( const std_msgs::Int16MultiArray& _msg)
{}
ros::Subscriber<std_msgs::Int16MultiArray> sub("robot_hand_control", &messageCb );
nh.subscribe(sub);
- Arduino部分完整代码
#include <FlexiTimer2.h>
#include <SCServo.h>
#include <SCSProtocol.h>
#include <ros.h>
#include <ArduinoHardware.h>
#include <std_msgs/MultiArrayLayout.h>
#include <std_msgs/MultiArrayDimension.h>
#include <std_msgs/Int16MultiArray.h>
///ROS初始化/
ros::NodeHandle nh;
///ROS订阅器/
int* Joint;
int MultiArray_length;
int count_length;
bool messageCb_judge,release_subCb_judge;
void messageCb( const std_msgs::Int16MultiArray& pos_msg)
{
MultiArray_length=((pos_msg.data_length)/6);
Joint[(pos_msg.data_length)];
count_length=MultiArray_length;
Joint=pos_msg.data;
//start send angle
FlexiTimer2::set(650, Data_send);
FlexiTimer2::start();
}
ros::Subscriber<std_msgs::Int16MultiArray> sub("robot_hand_control", &messageCb );
///总线舵机控制/
SCServo smServo;
SCServo scsServo;
///舵机参数//
#define J1_Angle_Init 0
#define J1_Min_Angle_Limit 10
#define J1_Max_Angle_Limit 1013
#define J2_Angle_Init 0
#define J2_Min_Angle_Limit 10
#define J2_Max_Angle_Limit 4090
#define J3_Angle_Init 0
#define J3_Min_Angle_Limit 10
#define J3_Max_Angle_Limit 4090
#define J4_Angle_Init 0
#define J4_Min_Angle_Limit 10
#define J4_Max_Angle_Limit 4090
#define J5_Angle_Init 0
#define J5_Min_Angle_Limit 10
#define J5_Max_Angle_Limit 1013
#define J6_Angle_Init 0
#define J6_Min_Angle_Limit 10
#define J6_Max_Angle_Limit 1013
//总线舵机中断函数/
int t = 600;
int count = 0;
void Data_send()
{
if(count<count_length)
{
messageCb_judge = true;
}
else
{
count=0;
MultiArray_length=0;
FlexiTimer2::stop();
}
}
///主函数/
void setup()
{
///ROS初始化/
nh.initNode();
nh.subscribe(sub);
///总线舵机初始化/
scsServo.End = 1;
smServo.End = 0;
Serial2.begin(1000000);
scsServo.pSerial = &Serial2;
smServo.pSerial = &Serial2;
delay(1000);
messageCb_judge = false;
///总线舵机角度初始化/
scsServo.EnableTorque(1, 1);
smServo.EnableTorque(2, 1);
smServo.EnableTorque(3, 1);
smServo.EnableTorque(4, 1);
scsServo.EnableTorque(5, 1);
scsServo.EnableTorque(6, 1);
scsServo.WritePos(1, J1_Angle_Init, 1000);
smServo.WritePos(2, J2_Angle_Init, 1000);
smServo.WritePos(3, J3_Angle_Init, 1000);
smServo.WritePos(4, J4_Angle_Init, 1000);
scsServo.WritePos(5, J5_Angle_Init, 1000);
scsServo.WritePos(6, J6_Angle_Init, 1000);
}
void loop()
{
if(messageCb_judge)
{
scsServo.WritePos(1,J1_Angle_Init- Joint[count*6+0], t);
smServo.WritePos(2, J2_Angle_Init-Joint[count*6+1], t);
smServo.WritePos(3, J3_Angle_Init+Joint[count*6+2], t);
smServo.WritePos(4, J4_Angle_Init-Joint[count*6+3], t);
scsServo.WritePos(5, J5_Angle_Init-Joint[count*6+4], t);
scsServo.WritePos(6, J6_Angle_Init-Joint[count*6+5], t);
play(1,Joint[count*6+0]);
play(2,Joint[count*6+1]);
play(3,Joint[count*6+2]);
play(4,Joint[count*6+3]);
play(5,Joint[count*6+4]);
play(6,Joint[count*6+5]);
count++;
messageCb_judge = false;
}
nh.spinOnce();
}