通过ROS控制真实机械臂(6)---move_group(编程接口)
1. 自定义消息，用于发布每个关节的状态。
GoalPoint.msg
int8 bus
float32 joint1
float32 joint2
float32 joint3
float32 joint4
float32 joint5
float32 joint6JointPos.msg
float32 joint1
float32 joint2
float32 joint3
float32 joint4
float32 joint5
float32 joint6
TraPoint.msg
int8 bus
int32 num_of_trapoint
float32[] trapoints
2.便于控制，通过Rviz的panel插件，制作一个控制的GUI界面，先在Qt Creater中完成界面设计, 通过调节Bus Interface的选项，可以选择按钮调节的速度，通过Mode选项，Continuous是即时发送数据，第二个用于购买的工业级机械臂的接口，第三个是最常用的通过Moveit规划的路径控制，通过调节每个Joint的数值为机械臂选取目标点，或者通过下面的五个预定义的位姿按钮，最后点击SendGoal控制机械臂的运动。最后一个是根据机械臂发布的关节状态信息，来改变Rviz和界面中的数据的。
接下来创建ROS包，这是通过Rviz插件的方法在ROS中编写GUI界面 ，最后编译生成的是.SO文件，没有启动的节点，只需要运行Rviz之后，调出panel插件即可。
redwall_arm_panel.h
#ifndef RedWallArm_PANEL_H
#define RedWallArm_PANEL_H
#include
#include
#include
#include
#include
#include
#include
namespace Ui {
class RedWallArmPanel;
}
namespace redwall_arm_panel
{
class RedWallArmPanel : public rviz::Panel
{
Q_OBJECT
public:
explicit RedWallArmPanel(QWidget *parent = 0);
~RedWallArmPanel();
public:
void initROS();
private Q_SLOTS:
void on_pbn_joint1Left_pressed();
void on_pbn_joint1Right_pressed();
void on_pbn_joint2Left_pressed();
void on_pbn_joint2Right_pressed();
void on_pbn_joint3Left_pressed();
void on_pbn_joint3Right_pressed();
void on_pbn_joint4Left_pressed();
void on_pbn_joint4Right_pressed();
void on_pbn_joint5Left_pressed();
void on_pbn_joint5Right_pressed();
void on_pbn_joint6Left_pressed();
void on_pbn_joint6Right_pressed();
void on_pbn_zero_clicked();
void on_pbn_classicPos1_clicked();
void on_pbn_classicPos2_clicked();
void on_pbn_classicPos3_clicked();
void on_pbn_classicPos4_clicked();
void on_pbn_sendGoal_clicked();
void on_rbx_pcan_clicked();
void on_rbx_tcp_clicked();
void on_rbx_continuous_clicked();
void on_rbx_goal_clicked();
void on_rbx_moveit_clicked();
void on_rbx_sync_clicked();
void on_pbn_setIO_clicked();
protected Q_SLOTS:
void sendCommand();
protected:
QTimer* m_timer;
// The ROS publisher/subscriber
ros::Publisher cmd1_publisher_;
ros::Publisher cmd2_publisher_;
ros::Publisher cmd3_publisher_;
ros::Publisher goal_publisher_;
sensor_msgs::JointState jointMsg;
std_msgs::Float32MultiArray joints;
redwall_arm_msgs::GoalPoint goalPoint;
ros::Subscriber subJointState_;
void jointStateCallback(const sensor_msgs::JointState::ConstPtr& msg);
int pointCompare(void);
// The ROS node handle.
ros::NodeHandle nh_;
float jointVelocities[6];
float jointPosition[6];
float lastJointPosition[6];
private:
Ui::RedWallArmPanel *ui;
float m_step;
float m_speed;
//int m_jointStateSync;
int m_controMode;
int m_busInterface;
};
}//end of redwall_arm_rviz_plugin
#endif // RedWallArm_PANEL_H
redwall_arm_panel.cpp
#include "redwall_arm_panel.h"
#include "ui_redwall_arm_panel.h"
#include
namespace redwall_arm_panel
{
RedWallArmPanel::RedWallArmPanel(QWidget *parent) :
rviz::Panel(parent),
ui(new Ui::RedWallArmPanel),
m_step(0.16),
m_speed(50.0),
m_controMode(3),
m_busInterface(1)
{
ui->setupUi(this);
ui->rbx_pcan->setChecked(false);
ui->rbx_tcp->setChecked(true);
ui->rbx_continuous->setChecked(false);
ui->rbx_goal->setChecked(false);
ui->rbx_moveit->setChecked(false);
ui->rbx_sync->setChecked(true);
ui->pbn_setIO->setEnabled(false);
ui->pbn_sendGoal->setEnabled(false);
ui->cbx_ioType->setMaxCount(1);
ui->cbx_ioType->addItem("Beaglebone");
ui->cbx_ioMode->setMaxCount(2);
ui->cbx_ioMode->addItem("ssh");
ui->cbx_ioMode->addItem("sftp");
ui->cbx_ioIndex->setMaxCount(2);
ui->cbx_ioIndex->addItem("192.168.7.2");
ui->cbx_ioIndex->addItem("192.168.0.2");
m_timer = new QTimer(this);
connect(m_timer, SIGNAL(timeout()), this, SLOT(sendCommand()));
initROS();
m_timer->start(10); // Start the timer.
}
RedWallArmPanel::~RedWallArmPanel()
{
delete ui;
}
void RedWallArmPanel::initROS()
{
joints.data.resize(6);
//cmd1_publisher_ = nh_.advertise("pcan_cmd", 1000);
cmd2_publisher_ = nh_.advertise("movej_cmd", 1000);
//cmd3_publisher_ = nh_.advertise("servoj_cmd", 1000);
goal_publisher_ = nh_.advertise("send_goal", 1);
subJointState_ = nh_.subscribe("joint_states", 1000, &RedWallArmPanel::jointStateCallback,this);
}
/* 避免重复发送相同位姿 */
int RedWallArmPanel::pointCompare(void)
{
// 如果相比前一刻时间位姿没有改变.则不发送
int ret = 0;
for(int i=0;i<6;i++)
{
if(fabs(joints.data[i]-lastJointPosition[i])>=0.000001)
{
ret = 1;
break;
}
}
return ret;
}
/* 订阅手臂发布joint_state消息, 更新Rviz中滑块的数据, m_controMode == 3 */
void RedWallArmPanel::jointStateCallback(const sensor_msgs::JointState::ConstPtr& msg)
{
// 初始位姿
lastJointPosition[0] = msg->position[0];
lastJointPosition[1] = msg->position[1];
lastJointPosition[2] = msg->position[2];
lastJointPosition[3] = msg->position[3];
lastJointPosition[4] = msg->position[4];
lastJointPosition[5] = msg->position[5];
if(m_controMode == 3)//sync with real robot
{
if(pointCompare())
{
joints.data[0] = lastJointPosition[0];
joints.data[1] = lastJointPosition[1];
joints.data[2] = lastJointPosition[2];
joints.data[3] = lastJointPosition[3];
joints.data[4] = lastJointPosition[4];
joints.data[5] = lastJointPosition[5];
ui->lb_joint1->setText(QString::number((joints.data[0]*180/M_PI),'f',6));
ui->lb_joint2->setText(QString::number((joints.data[1]*180/M_PI),'f',6 ));
ui->lb_joint3->setText(QString::number((joints.data[2]*180/M_PI),'f',6 ));
ui->lb_joint4->setText(QString::number((joints.data[3]*180/M_PI),'f',6 ));
ui->lb_joint5->setText(QString::number((joints.data[4]*180/M_PI),'f',6 ));
ui->lb_joint6->setText(QString::number((joints.data[5]*180/M_PI),'f',6 ));
}
}
}
/* 即时发送m_controMode == 0 */
void RedWallArmPanel::on_pbn_joint1Left_pressed()
{
if((m_controMode == 0)&&(m_busInterface == 0)) m_step = 0.16;
else m_step = 0.035;
joints.data[0] = joints.data[0] - m_step * m_speed/100 < -M_PI ?
joints.data[0] :
joints.data[0] - m_step * m_speed/100;
if(joints.data[0]< -3.05) joints.data[0] = -3.05;
ui->lb_joint1->setText(QString::number(joints.data[0]*180.0/M_PI, 'f', 6));
}
void RedWallArmPanel::on_pbn_joint1Right_pressed()
{
if((m_controMode == 0)&&(m_busInterface == 0)) m_step = 0.16;
else m_step = 0.035;
joints.data[0] = joints.data[0] + m_step * m_speed/100 > M_PI ?
joints.data[0] :
joints.data[0] + m_step * m_speed/100;
if(joints.data[0]> 3.05) joints.data[0] = 3.05;
ui->lb_joint1->setText(QString::number(joints.data[0]*180.0/M_PI, 'f', 6));
}
void RedWallArmPanel::on_pbn_joint2Left_pressed()
{
if((m_controMode == 0)&&(m_busInterface == 0)) m_step = 0.16;
else m_step = 0.035;
joints.data[1] = joints.data[1] - m_step * m_speed/100 < -M_PI ?
joints.data[1] :
joints.data[1] - m_step * m_speed/100;
if(joints.data[1]< -3.05) joints.data[1] = -3.05;
ui->lb_joint2->setText(QString::number(joints.data[1]*180.0/M_PI, 'f', 6));
}
void RedWallArmPanel::on_pbn_joint2Right_pressed()
{
if((m_controMode == 0)&&(m_busInterface == 0)) m_step = 0.16;
else m_step = 0.035;
joints.data[1] = joints.data[1] + m_step * m_speed/100 > M_PI ?
joints.data[1] :
joints.data[1] + m_step * m_speed/100;
if(joints.data[1]> 3.05) joints.data[1] = 3.05;
ui->lb_joint2->setText(QString::number(joints.data[1]*180.0/M_PI, 'f', 6));
}
void RedWallArmPanel::on_pbn_joint3Left_pressed()
{
if((m_controMode == 0)&&(m_busInterface == 0)) m_step = 0.16;
else m_step = 0.035;
joints.data[2] = joints.data[2] - m_step * m_speed/100 < -M_PI ?
joints.data[2] :
joints.data[2] - m_step * m_speed/100;
if(joints.data[2]< -3.05) joints.data[2] = -3.05;
ui->lb_joint3->setText(QString::number(joints.data[2]*180.0/M_PI, 'f', 6));
}
void RedWallArmPanel::on_pbn_joint3Right_pressed()
{
if((m_controMode == 0)&&(m_busInterface == 0)) m_step = 0.16;
else m_step = 0.035;
joints.data[2] = joints.data[2] + m_step * m_speed/100 > M_PI ?
joints.data[2] :
joints.data[2] + m_step * m_speed/100;
if(joints.data[2]> 3.05) joints.data[2] = 3.05;
ui->lb_joint3->setText(QString::number(joints.data[2]*180.0/M_PI, 'f', 6));
}
void RedWallArmPanel::on_pbn_joint4Left_pressed()
{
if((m_controMode == 0)&&(m_busInterface == 0)) m_step = 0.16;
else m_step = 0.035;
joints.data[3] = joints.data[3] - m_step * m_speed/100 < -M_PI ?
joints.data[3] :
joints.data[3] - m_step * m_speed/100;
if(joints.data[3]< -3.05) joints.data[3] = -3.05;
ui->lb_joint4->setText(QString::number(joints.data[3]*180.0/M_PI, 'f', 6));
}
void RedWallArmPanel::on_pbn_joint4Right_pressed()
{
if((m_controMode == 0)&&(m_busInterface == 0)) m_step = 0.16;
else m_step = 0.035;
joints.data[3] = joints.data[3] + m_step * m_speed/100 > M_PI ?
joints.data[3] :
joints.data[3] + m_step * m_speed/100;
if(joints.data[3]> 3.05) joints.data[3] = 3.05;
ui->lb_joint4->setText(QString::number(joints.data[3]*180.0/M_PI, 'f', 6));
}
void RedWallArmPanel::on_pbn_joint5Left_pressed()
{
if((m_controMode == 0)&&(m_busInterface == 0)) m_step = 0.16;
else m_step = 0.035;
joints.data[4] = joints.data[4] - m_step * m_speed/100 < -M_PI ?
joints.data[4] :
joints.data[4] - m_step * m_speed/100;
if(joints.data[4]< -3.05) joints.data[4] = -3.05;
ui->lb_joint5->setText(QString::number(joints.data[4]*180.0/M_PI, 'f', 6));
}
void RedWallArmPanel::on_pbn_joint5Right_pressed()
{
if((m_controMode == 0)&&(m_busInterface == 0)) m_step = 0.16;
else m_step = 0.035;
joints.data[4] = joints.data[4] + m_step * m_speed/100 > M_PI ?
joints.data[4] :
joints.data[4] + m_step * m_speed/100;
if(joints.data[4]> 3.05) joints.data[4] = 3.05;
ui->lb_joint5->setText(QString::number(joints.data[4]*180.0/M_PI, 'f', 6));
}
void RedWallArmPanel::on_pbn_joint6Left_pressed()
{
if((m_controMode == 0)&&(m_busInterface == 0)) m_step = 0.16;
else m_step = 0.035;
joints.data[5] = joints.data[5] - m_step * m_speed/100 < -M_PI ?
joints.data[5] :
joints.data[5] - m_step * m_speed/100;
if(joints.data[5]< -3.05) joints.data[5] = -3.05;
ui->lb_joint6->setText(QString::number(joints.data[5]*180.0/M_PI, 'f', 6));
}
void RedWallArmPanel::on_pbn_joint6Right_pressed()
{
if((m_controMode == 0)&&(m_busInterface == 0)) m_step = 0.16;
else m_step = 0.035;
joints.data[5] = joints.data[5] + m_step * m_speed/100 > M_PI ?
joints.data[5] :
joints.data[5] + m_step * m_speed/100;
if(joints.data[5]> 3.05) joints.data[5] = 3.05;
ui->lb_joint6->setText(QString::number(joints.data[5]*180.0/M_PI, 'f', 6));
}
/* moveit和按钮内容一样,所以m_controMode == 1||m_controMode == 2 */
void RedWallArmPanel::on_pbn_zero_clicked()
{
joints.data[0] = 0;
joints.data[1] = 0;
joints.data[2] = 0;
joints.data[3] = 0;
joints.data[4] = 0;
joints.data[5] = 0;
ui->lb_joint1->setText(QString::number(0.0, 'f', 6));
ui->lb_joint2->setText(QString::number(0.0, 'f', 6));
ui->lb_joint3->setText(QString::number(0.0, 'f', 6));
ui->lb_joint4->setText(QString::number(0.0, 'f', 6));
ui->lb_joint5->setText(QString::number(0.0, 'f', 6));
ui->lb_joint6->setText(QString::number(0.0, 'f', 6));
}
void RedWallArmPanel::on_pbn_classicPos1_clicked()
{
if((m_controMode == 1)||(m_controMode == 2))
{
joints.data[0] = 91*M_PI/180;
joints.data[1] = 52.3*M_PI/180;
joints.data[2] = -95*M_PI/180;
joints.data[3] = 40*M_PI/180;
joints.data[4] = 92.5*M_PI/180;
joints.data[5] = 123.8*M_PI/180;
ui->lb_joint1->setText(QString::number(91.0, 'f', 6));
ui->lb_joint2->setText(QString::number(52.3, 'f', 6));
ui->lb_joint3->setText(QString::number(-95.0, 'f', 6));
ui->lb_joint4->setText(QString::number(40.0, 'f', 6));
ui->lb_joint5->setText(QString::number(92.5, 'f', 6));
ui->lb_joint6->setText(QString::number(123.8, 'f', 6));
}
}
void RedWallArmPanel::on_pbn_classicPos2_clicked()
{
if((m_controMode == 1)||(m_controMode == 2))
{
joints.data[0] = -92*M_PI/180;
joints.data[1] = -61.7*M_PI/180;
joints.data[2] = 102*M_PI/180;
joints.data[3] = -24*M_PI/180;
joints.data[4] = -83.6*M_PI/180;
joints.data[5] = -101.1*M_PI/180;
ui->lb_joint1->setText(QString::number(-92.0, 'f', 6));
ui->lb_joint2->setText(QString::number(-61.7, 'f', 6));
ui->lb_joint3->setText(QString::number(102.0, 'f', 6));
ui->lb_joint4->setText(QString::number(-24.0, 'f', 6));
ui->lb_joint5->setText(QString::number(-83.6, 'f', 6));
ui->lb_joint6->setText(QString::number(-101.1, 'f', 6));
}
}
void RedWallArmPanel::on_pbn_classicPos3_clicked()
{
if((m_controMode == 1)||(m_controMode == 2))
{
joints.data[0] = 45*M_PI/180;
joints.data[1] = 45*M_PI/180;
joints.data[2] = -45*M_PI/180;
joints.data[3] = 45*M_PI/180;
joints.data[4] = 45*M_PI/180;
joints.data[5] = 145*M_PI/180;
ui->lb_joint1->setText(QString::number(45.0, 'f', 6));
ui->lb_joint2->setText(QString::number(45.0, 'f', 6));
ui->lb_joint3->setText(QString::number(-45.0, 'f', 6));
ui->lb_joint4->setText(QString::number(45.0, 'f', 6));
ui->lb_joint5->setText(QString::number(45.0, 'f', 6));
ui->lb_joint6->setText(QString::number(145.0, 'f', 6));
}
}
void RedWallArmPanel::on_pbn_classicPos4_clicked()
{
if((m_controMode == 1)||(m_controMode == 2))
{
joints.data[0] = 90*M_PI/180;
joints.data[1] = 45*M_PI/180;
joints.data[2] = -90*M_PI/180;
joints.data[3] = 45*M_PI/180;
joints.data[4] = 90*M_PI/180;
joints.data[5] = 135*M_PI/180;
ui->lb_joint1->setText(QString::number(90.0, 'f', 6));
ui->lb_joint2->setText(QString::number(90.0, 'f', 6));
ui->lb_joint3->setText(QString::number(-90.0, 'f', 6));
ui->lb_joint4->setText(QString::number(45.0, 'f', 6));
ui->lb_joint5->setText(QString::number(90.0, 'f', 6));
ui->lb_joint6->setText(QString::number(135.0, 'f', 6));
}
}
/* sendGoal点击按钮发布只针对m_controMode == 1||m_controMode == 2 */
/* 内容一样,但是发布的位置不一样 So... */
void RedWallArmPanel::on_pbn_sendGoal_clicked()
{
if(m_controMode == 1)
{
cmd2_publisher_.publish(joints); // std_msgs::Float32MultiArray joints;
// goalPoint.bus = m_busInterface; // 其实可以不用,但是后面可以区别发送到不同的板子
// goalPoint.joint1 = joints.data[0];
// goalPoint.joint2 = joints.data[1];
// goalPoint.joint3 = joints.data[2];
// goalPoint.joint4 = joints.data[3];
// goalPoint.joint5 = joints.data[4];
// goalPoint.joint6 = joints.data[5];
// goal_publisher_.publish(goalPoint); // 我目前用的是这个数据
}
else if(m_controMode == 2)
{
goalPoint.bus = m_busInterface; // 其实可以不用,但是后面可以区别发送到不同的板子
goalPoint.joint1 = joints.data[0];
goalPoint.joint2 = joints.data[1];
goalPoint.joint3 = joints.data[2];
goalPoint.joint4 = joints.data[3];
goalPoint.joint5 = joints.data[4];
goalPoint.joint6 = joints.data[5];
goal_publisher_.publish(goalPoint); // 我目前用的是这个数据
}
}
/* sendCommand则只针对 m_controMode == 0 */
void RedWallArmPanel::sendCommand()
{
//if( ros::ok()&&(cmd1_publisher_||cmd3_publisher_))
if( ros::ok()&&(goal_publisher_))
{
if(m_controMode == 0)
{
ROS_WARN("Continuous Send");
if(m_busInterface == 0)
{
//cmd1_publisher_.publish(joints);
goalPoint.bus = m_busInterface; // 其实可以不用,但是后面可以区别发送到不同的板子
goalPoint.joint1 = joints.data[0];
goalPoint.joint2 = joints.data[1];
goalPoint.joint3 = joints.data[2];
goalPoint.joint4 = joints.data[3];
goalPoint.joint5 = joints.data[4];
goalPoint.joint6 = joints.data[5];
goal_publisher_.publish(goalPoint); // 我目前用的是这个数据
}
else if(m_busInterface == 1)
{
//cmd3_publisher_.publish(joints);
goalPoint.bus = m_busInterface; // 其实可以不用,但是后面可以区别发送到不同的板子
goalPoint.joint1 = joints.data[0];
goalPoint.joint2 = joints.data[1];
goalPoint.joint3 = joints.data[2];
goalPoint.joint4 = joints.data[3];
goalPoint.joint5 = joints.data[4];
goalPoint.joint6 = joints.data[5];
goal_publisher_.publish(goalPoint); // 我目前用的是这个数据
}
}
}
}
/* 设置m_busInterface pcan的步长要大一点,速度更快 */
void RedWallArmPanel::on_rbx_pcan_clicked()
{
ui->rbx_pcan->setChecked(true);
ui->rbx_tcp->setChecked(false);
m_busInterface = 0;
}
void RedWallArmPanel::on_rbx_tcp_clicked()
{
ui->rbx_pcan->setChecked(false);
ui->rbx_tcp->setChecked(true);
m_busInterface = 1;
}
/* 不用点击滑块数据,仅改变滑块数据,就立刻发布出去 m_controlmode = 0 */
void RedWallArmPanel::on_rbx_continuous_clicked()
{
ui->rbx_continuous->setChecked(true);
ui->rbx_goal->setChecked(false);
ui->rbx_moveit->setChecked(false);
ui->rbx_sync->setChecked(false);
ui->pbn_sendGoal->setEnabled(false);
m_controMode = 0;
}
/* 通过按钮选择目标点 m_controlmode = 1 */
void RedWallArmPanel::on_rbx_goal_clicked()
{
ui->rbx_continuous->setChecked(false);
ui->rbx_goal->setChecked(true);
ui->rbx_moveit->setChecked(false);
ui->rbx_sync->setChecked(false);
ui->pbn_sendGoal->setEnabled(true);
m_controMode = 1;
}
/* 通过movite规划路径 m_controlmode = 2 */
void RedWallArmPanel::on_rbx_moveit_clicked()
{
ui->rbx_continuous->setChecked(false);
ui->rbx_goal->setChecked(false);
ui->rbx_moveit->setChecked(true);
ui->rbx_sync->setChecked(false);
ui->pbn_sendGoal->setEnabled(true);
m_controMode = 2;
}
/* sync with rosbot 模式,sendGoal按钮肯定是不能点击的 m_controlmode = 3 */
void RedWallArmPanel::on_rbx_sync_clicked()
{
ui->rbx_continuous->setChecked(false);
ui->rbx_goal->setChecked(false);
ui->rbx_moveit->setChecked(false);
ui->rbx_sync->setChecked(true);
ui->pbn_sendGoal->setEnabled(false);
m_controMode = 3;
}
}
#include
PLUGINLIB_EXPORT_CLASS(redwall_arm_panel::RedWallArmPanel,rviz::Panel)
程序中将该GUI命名为Teleop，打开Rviz之后，在上方的Panels中找到Teleop打开即可，就会出现设计的界面，可以随意调节它的位置。
3. 机械臂控制中不仅仅使用传统的消息msg或者服务svr了，而是具有反馈和状态的动作action，通过上篇文章，action分为服务器和客户端，当然要控制电机，我们还需要一个TCP的客户端和服务器，这两者不要弄混。action客户端的功能就是发布目标轨迹，服务端才是我们需要编写去接收action信息并转换成关节的控制信息。方便起见，我将tcp服务器和action服务器写到一个代码中，于是，当action服务端接收到信息之后处理成关节的轨迹点，然后通过tcp服务器直接发送给beaglebone上的tcp客户端，进而控制机械臂的运动
创建ROS包，里面不仅包含了action客户端服务器-----trajectory_client和trajectory_server,还有trajectory_goal和trajectory_gen，他们是用来与Rviz交互的，有了他们之后，当我们选择好了目标空间位置后，点击SendGoal按钮之后，真实机械臂和Rviz中的Urdf都会跟着运动。
目录中的4个client分别用于控制舵机机械手和步进电机机械手。代码太多而且底层硬件和控制板都不太一样，所以代码就不展示了,只将目前完成功能的程序上传了。