Arudino与机械臂电机的连接
需要的材料：
ArudinoUNO板子一个，面包板一个，杜邦线若干，小强机械臂自带的锂电池也需要用。
如下图
小强机械臂的6个伺服电机，每个电机有三根线，白色是信号线，连接到Arudino的每个端口，我选择的是A0，A1,A2,A3,A4,A5,.。
电机的红色线是正极，黑色线是负极，。电池的黑色线是负极，红色线是正极。
首先，我们将电池上的正负极对应插到面包板 上的正负极，然后再一次将电机的正负极线一次插到面包板的正负极。实现电机的并联。
最后，我们需要将面包板的负极和Arudino上的GND端口连接。完成这几步后，我们就完成了所有接线操作。
基于Moveit和Arudino的控制思路
重点理解前两个模块的内容，moveit实际上是通过action这种交互机制来控制机械臂的，并且moveit只提供action client，因此driver中的action server需要自己编写，两者的接口类型为FollowJointTrajectoryAction。
编写action server节点：myrobot_driver.cpp
综上，自建的/joint_states话题+文中提到的action server = myrobot_driver节点，即自定义的Driver节点。另外，我手头上的机械臂只是简单的舵机驱动，没有角度反馈，所以可以通过获取movie规划得到的最后一组数据作为机械臂运动输出后的实际状态（没有闭环反馈就是很low……），代码如下
/* myrobot_driver.cpp */
#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include <control_msgs/FollowJointTrajectoryAction.h>
#include <control_msgs/FollowJointTrajectoryActionGoal.h>
#include <control_msgs/FollowJointTrajectoryActionResult.h>
#include <sensor_msgs/JointState.h>
#include <std_msgs/Float64.h>
#include <iostream>
#include <string.h>
#include <stdio.h>
#include <vector>
/*------只是arm group部分，因此只有6个舵机-----*/
//换算舵机PWM值与弧度之间的关系
//舵机运动范围PWM500-2500，对应角度0°-270°，中间状态为1500（设定为偏置）
//关节弧度范围-2.36-2.36，0rad对应舵机中间状态1500
//换算得到每变化1rad，PWM变化423
#define scaler 423
#define offset 1500
using namespace std;
class FollowJointTrajectoryAction
{
protected:
sensor_msgs::JointState js;
std_msgs::Float64 joint1_pos, joint2_pos, joint3_pos, joint4_pos, joint5_pos, joint6_pos;
ros::NodeHandle nh;
std::string action_name_;
ros::Publisher pub_joint;//给move_group识别的publisher，代替joint_state_publisher，发布joint_states
ros::Publisher pub_joint1;//给下位机arduino识别的publiser
ros::Publisher pub_joint2;//同上
ros::Publisher pub_joint3;//同上
ros::Publisher pub_joint4;//同上
ros::Publisher pub_joint5;//同上
ros::Publisher pub_joint6;//同上------
//与moveit中action client通讯的action server
actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> as_;
control_msgs::FollowJointTrajectoryActionResult result_;
control_msgs::FollowJointTrajectoryActionGoal goal_;
public:
FollowJointTrajectoryAction(std::string name) :
as_(nh, name, false),
action_name_(name)
{
as_.registerGoalCallback(boost::bind(&FollowJointTrajectoryAction::goalCB, this));
as_.registerPreemptCallback(boost::bind(&FollowJointTrajectoryAction::preemptCB, this));
as_.start();
pub_joint = nh.advertise<sensor_msgs::JointState>("/joint_states", 10);
pub_joint1 = nh.advertise<std_msgs::Float64>("joint1_value", 100);
pub_joint2 = nh.advertise<std_msgs::Float64>("joint2_value", 100);
pub_joint3 = nh.advertise<std_msgs::Float64>("joint3_value", 100);
pub_joint4 = nh.advertise<std_msgs::Float64>("joint4_value", 100);
pub_joint5 = nh.advertise<std_msgs::Float64>("joint5_value", 100);
pub_joint6 = nh.advertise<std_msgs::Float64>("joint6_value", 100);//--------
// ros::Rate loop_rate(10);
js.name.resize(6);
js.position.resize(6);
//名字要与关节定义的名字一致
js.name[0] = "joint1";
js.name[1] = "joint2";
js.name[2] = "joint3";
js.name[3] = "joint4";
js.name[4] = "joint5";
js.name[5] = "joint6";//----------------------
ROS_INFO("-------action start!-------");
}
~FollowJointTrajectoryAction(void)
{
}
void goalCB()
{
ROS_INFO("-------goal is receiveing!-------");
std::vector<trajectory_msgs::JointTrajectoryPoint> points_;
double points_end[6];
double Pos_length;
if (as_.isNewGoalAvailable()){
js.position.clear();
points_ = as_.acceptNewGoal()->trajectory.points;
Pos_length = points_.size();
for (int i = 0; i < 6; i++){
//假设v是一个vector对象,v.at(n)和v[n]是一样的
//但是前者会检查是否越界，后者不会
points_end[i] = points_.at(Pos_length - 1).positions[i];
js.position.push_back(points_end[i]);
}
js.header.stamp = ros::Time::now();
//向move_group节点发布规划得到的关节值
pub_joint.publish(js);
//向下位机arduino节点发布规划得到的关节值，直接得到舵机PWM值
//舵机2需要反向
joint1_pos.data = js.position[0] * scaler + offset;
joint2_pos.data = -js.position[1] * scaler + offset;
joint3_pos.data = js.position[2] * scaler + offset;
joint4_pos.data = js.position[3] * scaler + offset;
joint5_pos.data = js.position[4] * scaler + offset;
joint6_pos.data = js.position[5] * scaler + offset;//-----------------------
pub_joint1.publish(joint1_pos);
pub_joint2.publish(joint2_pos);
pub_joint3.publish(joint3_pos);
pub_joint4.publish(joint4_pos);
pub_joint5.publish(joint5_pos);
pub_joint6.publish(joint6_pos);
ROS_INFO("-------goal is receive done!-------");
}else{
ROS_INFO("-------goal is not availabel!-------");
}
control_msgs::FollowJointTrajectoryResult result;
result.error_code = 0;
as_.setSucceeded(result);
}
void preemptCB()
{
ROS_INFO("%s: Preempted", action_name_.c_str());
as_.setPreempted();
}
};
int main(int argc, char *argv[])
{
ros::init(argc, argv, "myrobot_driver");
FollowJointTrajectoryAction followjointtrajectory("LOBOT/follow_joint_trajectory");//名称要与yaml配置一致
ros::spin();
return 0;
}
节点电监视器myrobot_sub.cpp
编写一个简单的myrobot_sub.cpp
为了省事，直接撸一个订阅节点，把myrobot_driver.cpp中写的jointX_value全部订阅监听一道，看算的舵机PWM值与弧度值之间是不是正确对应关系，也方便后面测试舵机。
#include <ros/ros.h>
#include <std_msgs/MultiArrayLayout.h>
#include <std_msgs/String.h>
#include <std_msgs/MultiArrayDimension.h>
#include <std_msgs/Int16MultiArray.h>
#include <sensor_msgs/JointState.h>
#include <std_msgs/Float64.h>
/*------只是arm group部分，因此只有五个舵机-----*/
#define scaler 423
#define offset 1500
float jointval[6] = {0.0, 0.0, 0.0, 0.0, 0.0,0.0};//------
void callback1(const std_msgs::Float64& pos_msg)
{
jointval[0] = pos_msg.data ;//舵机1
ROS_INFO("[%f] ", jointval[0]);
}
void callback2(const std_msgs::Float64& pos_msg)
{
jointval[1] = pos_msg.data ;//舵机2
ROS_INFO("[%f] ", jointval[1]);
}
void callback3(const std_msgs::Float64& pos_msg)
{
jointval[2] = pos_msg.data ;//舵机3
ROS_INFO("[%f] ", jointval[2]);
}
void callback4(const std_msgs::Float64& pos_msg)
{
jointval[3] = pos_msg.data;//舵机4
ROS_INFO("[%f] ", jointval[3]);
}
void callback5(const std_msgs::Float64&pos_msg)
{
jointval[4] = pos_msg.data ;//舵机5
ROS_INFO("[%f] ", jointval[4]);
ROS_INFO("-----------done successfully-----------");
}
void callback6(const std_msgs::Float64&pos_msg)
{
jointval[5] = pos_msg.data ;//舵机6-----------------------------
ROS_INFO("[%f] ", jointval[5]);
ROS_INFO("-----------done successfully-----------");
}
int main(int argc, char *argv[])
{
/* code for main function */
ros::init(argc, argv, "ListenJointValue");
ros::NodeHandle nh;
ros::Subscriber sub1 = nh.subscribe("joint1_value", 1000, callback1);
ros::Subscriber sub2 = nh.subscribe("joint2_value", 1000, callback2);
ros::Subscriber sub3 = nh.subscribe("joint3_value", 1000, callback3);
ros::Subscriber sub4 = nh.subscribe("joint4_value", 1000, callback4);
ros::Subscriber sub5 = nh.subscribe("joint5_value", 1000, callback5);
ros::Subscriber sub6 = nh.subscribe("joint6_value", 1000, callback6);//----------------
ros::spin();
return 0;
}
arduino端节点
模仿上面的myrobot_sub.cpp可以很快的写出arduino程序，只需要加个舵机初始化和驱动程序即可，简单来说就是订阅监听+驱动。
这个节点我们直接烧写到Arudino中，
aArudino节点的程序
#include <ros.h>
#include <std_msgs/Int16.h>
#include <std_msgs/Float64.h>
#include <ArduinoHardware.h>
#include <Servo.h>
#define USE_USBCON
#define SERVO_NUM 6
#define init 1500
#define SERVO_TIME_PERIOD 20 //每隔20ms处理一次（累加）舵机的PWM增量
Servo myservo[SERVO_NUM];
const byte servo_pin[SERVO_NUM] = {A5, A2, A3, A0, A1,A4};//------------------
float jointval[SERVO_NUM] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};//-----------------
bool check = false;//check为true才表示订阅到消息
typedef struct
{ //舵机结构体变量声明
unsigned int aim = 1500; //舵机目标值
float cur = 1500.0; //舵机当前值
float inc= 8.48; //舵机值增量，以20ms为周期
}duoji_struct;
duoji_struct servo_do[SERVO_NUM]; //用结构体变量声明一个舵机变量组
ros::NodeHandle nh;
void joint1_callback(const std_msgs::Float64& msg)
{
jointval[0] = msg.data;
check = true;
}
void joint2_callback(const std_msgs::Float64& msg)
{
jointval[1] = msg.data;
}
void joint3_callback(const std_msgs::Float64& msg)
{
jointval[2] = msg.data;
}
void joint4_callback(const std_msgs::Float64& msg)
{
jointval[3] = msg.data;
}
void joint5_callback(const std_msgs::Float64& msg)
{
jointval[4] = msg.data;
}
void joint6_callback(const std_msgs::Float64& msg)
{
jointval[5] = msg.data;//------------------------------------------
}
ros::Subscriber<std_msgs::Float64> sub1("joint1_value", joint1_callback);
ros::Subscriber<std_msgs::Float64> sub2("joint2_value", joint2_callback);
ros::Subscriber<std_msgs::Float64> sub3("joint3_value", joint3_callback);
ros::Subscriber<std_msgs::Float64> sub4("joint4_value", joint4_callback);
ros::Subscriber<std_msgs::Float64> sub5("joint5_value", joint5_callback);
ros::Subscriber<std_msgs::Float64> sub6("joint6_value", joint6_callback);//-------------
void AttachServosAndInit()
{
for (byte i = 0; i < SERVO_NUM; i++)
{
myservo[i].attach(servo_pin[i]);
myservo[i].writeMicroseconds(init);
}
}
void subscribeToAll()
{
nh.subscribe(sub1);
nh.subscribe(sub2);
nh.subscribe(sub3);
nh.subscribe(sub4);
nh.subscribe(sub5);
nh.subscribe(sub6);//------------------------
}
/*
时间处理函数，第一个参数是上一次处理时间点， 第二个参数是处理时间间隔，
到达时间间隔返回0，否则返回1
*/
bool handleTimePeriod( unsigned long *ptr_time, unsigned int time_period) {
if((millis() - *ptr_time) < time_period) {//millis()返回Arduino开始运行当前程序以来经历的毫秒数
return 1;
} else{
*ptr_time = millis();
return 0;
}
}
//解析订阅得到的PWM
void analyJoint()
{
for (byte i = 0; i < SERVO_NUM; i++)
servo_do[i].aim = (int)jointval[i];
}
//舵机PWM增量处理函数，每隔SERVO_TIME_PERIOD毫秒处理一次，这样就实现了舵机的连续控制
void handleServo()
{
static unsigned long systick_ms_bak = 0;
if(handleTimePeriod(&systick_ms_bak, SERVO_TIME_PERIOD))return; //20ms处理一次，不到20ms则返回不处理
for(byte i = 0; i < SERVO_NUM; i++) {
if(abs( servo_do[i].aim - servo_do[i].cur) <= abs (servo_do[i].inc) ) {//这里就体现了这个程序的精度，SERVO_TIME_PERIOD越小精度越高
myservo[i].writeMicroseconds(servo_do[i].aim);
} else {
if (servo_do[i].aim < servo_do[i].cur )
servo_do[i].cur -= servo_do[i].inc;
else
servo_do[i].cur += servo_do[i].inc;
myservo[i].writeMicroseconds((int)servo_do[i].cur);
}
}
}
void setup(){
nh.initNode();
subscribeToAll();
AttachServosAndInit();
}
void loop(){
nh.spinOnce();
analyJoint();
if (check){//保证起始状态为中间位置
handleServo();
}
}
需要添加的库
我们可以看到，在文件的开头有一个ros.h，如果我们不安装对应的文件库，在编译的时候就会报错。
根据这个教程 把文件需要的库安装了就可以了。
运行代码
首先，需要编译代码。我是使用catkin build编译空间的，当然你也可以用catkin_make编译。
catkin build
source ~/ws_moveit/devel/setup.bash
这边及得吧ws_moveit改成你的工作空间的名字
下面就是在工作空间下打开终端
第一，打开Action server节点
source ~/ws_moveit/devel/setup.bash
rosrun probot_demo myrobot_driver
第二，打开另一个终端，打开监听器。这个是可选的。可以不打开
source ~/ws_moveit/devel/setup.bash
rosrun probot_demo myrobot_sub
第三，打开新的终端，启动rviz，启动 机械臂模型
source ~/ws_moveit/devel/setup.bash
roslaunch LOBOT_moveit_config demo.launch
再打开一个终端，启动 USB串口
source ~/ws_moveit/devel/setup.bash
rosrun rosserial_python serial_node.py /dev/ttyUSB0
最后，启动一个机械臂正像运动学
source ~/ws_moveit/devel/setup.bash
rosrun probot_demo moveit_fk_demo
上面的步骤，打击肯定觉得很烦，做没需要打开这么多终端？
所以，我们将上面的所有启动程序写在一个launch文件中。
文件命名为startup_robot.launch
<?xml version="1.0"?>
<launch>
<node pkg="probot_demo" name="qidong_robot" type="myrobot_driver" />
<include file="$(find LOBOT_moveit_config)/launch/demo.launch"/>
<node pkg="rosserial_python" name="qidong_chuankou" type="serial_node.py" args="/dev/ttyUSB0"/>
</launch>
把这个文件放到其中一共功能包下面，创建一个luanch文件保存它。
我是放在probot_demo文件夹下面的：
在这个时候，主需要启动下面命令
roslaunch probot_demo startup_robot.launch
然后你可以在rviz下面控制机械臂运动，plan and excute
也可以执行我们的demo
即打开一个新的文件夹
source ~/ws_moveit/devel/setup.bash
rosrun probot_demo moveit_fk_demo
这也是可以的。
效果如下
LOBOT9
评论（0）
您还未登录，请登录后发表或查看评论