ros下编写QT界面功能包读取和驱动TX90及OptoForce力传感器
1.编写qt功能包
catkin_create_qt_pkg zlc_robot
生成的文件如下
对自动生成的功能包做修改，重命名ui文件，添加一个文本uic,里面写入如下命令，每次更改后运行该命令将自动生成头文件
uic zlc_main_window.ui -o ui_zlc_main_window.h
改后的界面如下：
运行效果如图：
上述资源见链接：https://download.csdn.net/download/weixin_42355349/11205369
2.qt多线程实现（不是很规范）
zlc_thread.h文件如下：
#ifndef ZLC_THREAD_H
#define ZLC_THREAD_H
#include<QThread>
#include <vector>
#include "std_msgs/String.h"
#include "sensor_msgs/JointState.h"
class zlc_thread : public QThread
{
public:
zlc_thread();
~zlc_thread();
void closeThread();
void emittoui();
protected:
virtual void run();
public:
bool isjointstate_open;
bool issensorstate_open;
// bool isjoint_control;
bool istimer;
int threadmode;
float step_size;
float px;
float py;
float pz;
float ow;
float ox;
float oy;
float oz;
float j1;
float j2;
float j3;
float j4;
float j5;
float j6;
double pos[6];
double vel[6];
double acc[6];
double sensor_F[3];
double sensor_T[3];
private:
volatile bool isStop; //isStop是易失性变量，需要用volatile进行申明
};
#endif // ZLC_THREAD_H
zlc_thread.cpp文件如下：
#include "zlc_thread.h"
#include <QDebug>
#include <QMutex>
#include <iostream>
#include <Eigen/Eigen>
#include <stdlib.h>
#include <Eigen/Geometry> //eigeｎ
#include <Eigen/Core>
#include <vector>
#include <math.h>
#include <ros/ros.h>
#include <ros/console.h>
#include <rviz/panel.h> //plugin基类的头文件
#include <Eigen/Eigen>
#include <vector>
#include <memory.h>
#include <moveit/move_group_interface/move_group.h>
#include <moveit/robot_state/robot_state.h>
#include <iostream>
#include <fstream>
//ADD
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/DisplayTrajectory.h>
#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h>
#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit/planning_request_adapter/planning_request_adapter.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>
#include <moveit/trajectory_processing/trajectory_tools.h>
#include <moveit_visual_tools/moveit_visual_tools.h>
#include <geometry_msgs/PointStamped.h>
#include "../../etherdaq_ros-master/include/etherdaq_driver/etherdaq_driver.h"
#include <stdlib.h>
#include <stdio.h>
#include "std_msgs/Bool.h"
#include "ros/ros.h"
#include "geometry_msgs/WrenchStamped.h"
#define GOHOME 0
#define J1ADD 1
#define J1DEC 2
#define J2ADD 3
#define J2DEC 4
#define J3ADD 5
#define J3DEC 6
#define J4ADD 7
#define J4DEC 8
#define J5ADD 9
#define J5DEC 10
#define J6ADD 11
#define J6DEC 12
#define PI 3.14159265358979323846
using namespace std;
using namespace optoforce_etherdaq_driver;
zlc_thread* pthread;
zlc_thread::zlc_thread()
{
isStop = false;
istimer = false;
isjointstate_open = false;
issensorstate_open = false;
// isjoint_control = false;
pthread = this;
}
zlc_thread::~zlc_thread()
{
}
void zlc_thread::closeThread()
{
isStop = true;
}
void zlc_thread::emittoui()
{
}
extern zlc_thread* pthread;
void jointstatesCallback(const sensor_msgs::JointStateConstPtr& msg)
{
pthread->pos[0] = msg->position[0];
pthread->pos[1] = msg->position[1];
pthread->pos[2] = msg->position[2];
pthread->pos[3] = msg->position[3];
pthread->pos[4] = msg->position[4];
pthread->pos[5] = msg->position[5];
}
void sensordatasCallback(const geometry_msgs::WrenchStamped& msg)
{
pthread->sensor_F[0] = msg.wrench.force.x;
pthread->sensor_F[1] = msg.wrench.force.y;
pthread->sensor_F[2] = msg.wrench.force.z;
pthread->sensor_T[0] = msg.wrench.torque.x;
pthread->sensor_T[1] = msg.wrench.torque.y;
pthread->sensor_T[2] = msg.wrench.torque.z;
}
void zlc_thread::run()
{
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();
static const std::string PLANNING_GROUP = "arm";
moveit::planning_interface::MoveGroup group(PLANNING_GROUP);
// 原始指针经常被用来指代计划组以提高性能。
const robot_state::JointModelGroup *joint_model_group =
group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
std::vector <double> joint;
if(isjointstate_open == true)
{
ros::Subscriber sub = node_handle.subscribe("/joint_states", 1000, jointstatesCallback);
while(1)
{
sleep(1); //1ms
}
}
else
{
switch (threadmode)
{
case GOHOME:
{
joint.clear();
joint.push_back(0*PI/180);
joint.push_back(0*PI/180);
joint.push_back(90*PI/180);
joint.push_back(0*PI/180);
joint.push_back(45*PI/180);
joint.push_back(0*PI/180);
group.setJointValueTarget(joint);
//group.setMaxVelocityScalingFactor(velscale);
// 进行运动规划，计算机器人移动到目标的运动轨迹，此时只是计算出轨迹，并不会控制机械臂运动
moveit::planning_interface::MoveGroup::Plan my_plan;
moveit::planning_interface::MoveItErrorCode success = group.plan(my_plan);
//让机械臂按照规划的轨迹开始运动。
if(success)
group.execute(my_plan);
}
break;
case J1ADD:
{
joint.clear();
joint.push_back(group.getCurrentJointValues().at(0)+(step_size*PI/180));
joint.push_back(group.getCurrentJointValues().at(1));
joint.push_back(group.getCurrentJointValues().at(2));
joint.push_back(group.getCurrentJointValues().at(3));
joint.push_back(group.getCurrentJointValues().at(4));
joint.push_back(group.getCurrentJointValues().at(5));
// group.setMaxVelocityScalingFactor(velscale);
group.setJointValueTarget(joint);
// group.move();
// 进行运动规划，计算机器人移动到目标的运动轨迹，此时只是计算出轨迹，并不会控制机械臂运动
moveit::planning_interface::MoveGroup::Plan my_plan;
moveit::planning_interface::MoveItErrorCode success = group.plan(my_plan);
ROS_INFO("Visualizing plan 1 (joint goal) %s",success?"":"FAILED");
//让机械臂按照规划的轨迹开始运动。
if(success)
group.execute(my_plan);
}
break;
case J1DEC:
{
joint.clear();
joint.push_back(group.getCurrentJointValues().at(0)-(step_size*PI/180));
joint.push_back(group.getCurrentJointValues().at(1));
joint.push_back(group.getCurrentJointValues().at(2));
joint.push_back(group.getCurrentJointValues().at(3));
joint.push_back(group.getCurrentJointValues().at(4));
joint.push_back(group.getCurrentJointValues().at(5));
// group.setMaxVelocityScalingFactor(velscale);
group.setJointValueTarget(joint);
// group.move();
// 进行运动规划，计算机器人移动到目标的运动轨迹，此时只是计算出轨迹，并不会控制机械臂运动
moveit::planning_interface::MoveGroup::Plan my_plan;
moveit::planning_interface::MoveItErrorCode success = group.plan(my_plan);
ROS_INFO("Visualizing plan 1 (joint goal) %s",success?"":"FAILED");
//让机械臂按照规划的轨迹开始运动。
if(success)
group.execute(my_plan);
}
break;
case J2ADD:
{
joint.clear();
joint.push_back(group.getCurrentJointValues().at(0));
joint.push_back(group.getCurrentJointValues().at(1)+(step_size*PI/180));
joint.push_back(group.getCurrentJointValues().at(2));
joint.push_back(group.getCurrentJointValues().at(3));
joint.push_back(group.getCurrentJointValues().at(4));
joint.push_back(group.getCurrentJointValues().at(5));
// group.setMaxVelocityScalingFactor(velscale);
group.setJointValueTarget(joint);
// group.move();
// 进行运动规划，计算机器人移动到目标的运动轨迹，此时只是计算出轨迹，并不会控制机械臂运动
moveit::planning_interface::MoveGroup::Plan my_plan;
moveit::planning_interface::MoveItErrorCode success = group.plan(my_plan);
ROS_INFO("Visualizing plan 1 (joint goal) %s",success?"":"FAILED");
//让机械臂按照规划的轨迹开始运动。
if(success)
group.execute(my_plan);
}
break;
case J2DEC:
{
joint.clear();
joint.push_back(group.getCurrentJointValues().at(0));
joint.push_back(group.getCurrentJointValues().at(1)-(step_size*PI/180));
joint.push_back(group.getCurrentJointValues().at(2));
joint.push_back(group.getCurrentJointValues().at(3));
joint.push_back(group.getCurrentJointValues().at(4));
joint.push_back(group.getCurrentJointValues().at(5));
// group.setMaxVelocityScalingFactor(velscale);
group.setJointValueTarget(joint);
// group.move();
// 进行运动规划，计算机器人移动到目标的运动轨迹，此时只是计算出轨迹，并不会控制机械臂运动
moveit::planning_interface::MoveGroup::Plan my_plan;
moveit::planning_interface::MoveItErrorCode success = group.plan(my_plan);
ROS_INFO("Visualizing plan 1 (joint goal) %s",success?"":"FAILED");
//让机械臂按照规划的轨迹开始运动。
if(success)
group.execute(my_plan);
}
break;
case J3ADD:
{
joint.clear();
joint.push_back(group.getCurrentJointValues().at(0));
joint.push_back(group.getCurrentJointValues().at(1));
joint.push_back(group.getCurrentJointValues().at(2)+(step_size*PI/180));
joint.push_back(group.getCurrentJointValues().at(3));
joint.push_back(group.getCurrentJointValues().at(4));
joint.push_back(group.getCurrentJointValues().at(5));
// group.setMaxVelocityScalingFactor(velscale);
group.setJointValueTarget(joint);
// group.move();
// 进行运动规划，计算机器人移动到目标的运动轨迹，此时只是计算出轨迹，并不会控制机械臂运动
moveit::planning_interface::MoveGroup::Plan my_plan;
moveit::planning_interface::MoveItErrorCode success = group.plan(my_plan);
ROS_INFO("Visualizing plan 1 (joint goal) %s",success?"":"FAILED");
//让机械臂按照规划的轨迹开始运动。
if(success)
group.execute(my_plan);
}
break;
case J3DEC:
{
joint.clear();
joint.push_back(group.getCurrentJointValues().at(0));
joint.push_back(group.getCurrentJointValues().at(1));
joint.push_back(group.getCurrentJointValues().at(2)-(step_size*PI/180));
joint.push_back(group.getCurrentJointValues().at(3));
joint.push_back(group.getCurrentJointValues().at(4));
joint.push_back(group.getCurrentJointValues().at(5));
// group.setMaxVelocityScalingFactor(velscale);
group.setJointValueTarget(joint);
//group.move();
// 进行运动规划，计算机器人移动到目标的运动轨迹，此时只是计算出轨迹，并不会控制机械臂运动
moveit::planning_interface::MoveGroup::Plan my_plan;
moveit::planning_interface::MoveItErrorCode success = group.plan(my_plan);
ROS_INFO("Visualizing plan 1 (joint goal) %s",success?"":"FAILED");
//让机械臂按照规划的轨迹开始运动。
if(success)
group.execute(my_plan);
}
break;
case J4ADD:
{
joint.clear();
joint.push_back(group.getCurrentJointValues().at(0));
joint.push_back(group.getCurrentJointValues().at(1));
joint.push_back(group.getCurrentJointValues().at(2));
joint.push_back(group.getCurrentJointValues().at(3)+(step_size*PI/180));
joint.push_back(group.getCurrentJointValues().at(4));
joint.push_back(group.getCurrentJointValues().at(5));
// group.setMaxVelocityScalingFactor(velscale);
group.setJointValueTarget(joint);
//group.move();
// 进行运动规划，计算机器人移动到目标的运动轨迹，此时只是计算出轨迹，并不会控制机械臂运动
moveit::planning_interface::MoveGroup::Plan my_plan;
moveit::planning_interface::MoveItErrorCode success = group.plan(my_plan);
ROS_INFO("Visualizing plan 1 (joint goal) %s",success?"":"FAILED");
//让机械臂按照规划的轨迹开始运动。
if(success)
group.execute(my_plan);
}
break;
case J4DEC:
{
joint.clear();
joint.push_back(group.getCurrentJointValues().at(0));
joint.push_back(group.getCurrentJointValues().at(1));
joint.push_back(group.getCurrentJointValues().at(2));
joint.push_back(group.getCurrentJointValues().at(3)-(step_size*PI/180));
joint.push_back(group.getCurrentJointValues().at(4));
joint.push_back(group.getCurrentJointValues().at(5));
// group.setMaxVelocityScalingFactor(velscale);
group.setJointValueTarget(joint);
//group.move();
// 进行运动规划，计算机器人移动到目标的运动轨迹，此时只是计算出轨迹，并不会控制机械臂运动
moveit::planning_interface::MoveGroup::Plan my_plan;
moveit::planning_interface::MoveItErrorCode success = group.plan(my_plan);
ROS_INFO("Visualizing plan 1 (joint goal) %s",success?"":"FAILED");
//让机械臂按照规划的轨迹开始运动。
if(success)
group.execute(my_plan);
}
break;
case J5ADD:
{
joint.clear();
joint.push_back(group.getCurrentJointValues().at(0));
joint.push_back(group.getCurrentJointValues().at(1));
joint.push_back(group.getCurrentJointValues().at(2));
joint.push_back(group.getCurrentJointValues().at(3));
joint.push_back(group.getCurrentJointValues().at(4)+(step_size*PI/180));
joint.push_back(group.getCurrentJointValues().at(5));
// group.setMaxVelocityScalingFactor(velscale);
group.setJointValueTarget(joint);
// group.move();
// 进行运动规划，计算机器人移动到目标的运动轨迹，此时只是计算出轨迹，并不会控制机械臂运动
moveit::planning_interface::MoveGroup::Plan my_plan;
moveit::planning_interface::MoveItErrorCode success = group.plan(my_plan);
ROS_INFO("Visualizing plan 1 (joint goal) %s",success?"":"FAILED");
//让机械臂按照规划的轨迹开始运动。
if(success)
group.execute(my_plan);
}
break;
case J5DEC:
{
joint.clear();
joint.push_back(group.getCurrentJointValues().at(0));
joint.push_back(group.getCurrentJointValues().at(1));
joint.push_back(group.getCurrentJointValues().at(2));
joint.push_back(group.getCurrentJointValues().at(3));
joint.push_back(group.getCurrentJointValues().at(4)-(step_size*PI/180));
joint.push_back(group.getCurrentJointValues().at(5));
// group.setMaxVelocityScalingFactor(velscale);
group.setJointValueTarget(joint);
//group.move();
// 进行运动规划，计算机器人移动到目标的运动轨迹，此时只是计算出轨迹，并不会控制机械臂运动
moveit::planning_interface::MoveGroup::Plan my_plan;
moveit::planning_interface::MoveItErrorCode success = group.plan(my_plan);
ROS_INFO("Visualizing plan 1 (joint goal) %s",success?"":"FAILED");
//让机械臂按照规划的轨迹开始运动。
if(success)
group.execute(my_plan);
}
break;
case J6ADD:
{
joint.clear();
joint.push_back(group.getCurrentJointValues().at(0));
joint.push_back(group.getCurrentJointValues().at(1));
joint.push_back(group.getCurrentJointValues().at(2));
joint.push_back(group.getCurrentJointValues().at(3));
joint.push_back(group.getCurrentJointValues().at(4));
joint.push_back(group.getCurrentJointValues().at(5)+(step_size*PI/180));
// group.setMaxVelocityScalingFactor(velscale);
group.setJointValueTarget(joint);
//group.move();
// 进行运动规划，计算机器人移动到目标的运动轨迹，此时只是计算出轨迹，并不会控制机械臂运动
moveit::planning_interface::MoveGroup::Plan my_plan;
moveit::planning_interface::MoveItErrorCode success = group.plan(my_plan);
ROS_INFO("Visualizing plan 1 (joint goal) %s",success?"":"FAILED");
//让机械臂按照规划的轨迹开始运动。
if(success)
group.execute(my_plan);
}
break;
case J6DEC:
{
joint.clear();
joint.push_back(group.getCurrentJointValues().at(0));
joint.push_back(group.getCurrentJointValues().at(1));
joint.push_back(group.getCurrentJointValues().at(2));
joint.push_back(group.getCurrentJointValues().at(3));
joint.push_back(group.getCurrentJointValues().at(4));
joint.push_back(group.getCurrentJointValues().at(5)-(step_size*PI/180));
// group.setMaxVelocityScalingFactor(velscale);
group.setJointValueTarget(joint);
// group.move();
// 进行运动规划，计算机器人移动到目标的运动轨迹，此时只是计算出轨迹，并不会控制机械臂运动
moveit::planning_interface::MoveGroup::Plan my_plan;
moveit::planning_interface::MoveItErrorCode success = group.plan(my_plan);
ROS_INFO("Visualizing plan 1 (joint goal) %s",success?"":"FAILED");
//让机械臂按照规划的轨迹开始运动。
if(success)
group.execute(my_plan);
}
break;
default:
break;
}
}
if(issensorstate_open == true)
{
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
// ros::Rate loop_rate(1000); // The loop rate
spinner.start();
ros::Subscriber sub_new = node_handle.subscribe("ethdaq_data", 1000, sensordatasCallback);
while(1)
{
sleep(1); //1ms
// ros::spinOnce();
//loop_rate.sleep();
}
}
}
main_window.cpp如下：
/**
* @file /src/main_window.cpp
*
* @brief Implementation for the qt gui.
*
* @date February 2011
**/
/*****************************************************************************
** Includes
*****************************************************************************/
#include <QtGui>
#include <QMessageBox>
#include <iostream>
#include "../include/zlc_robot/main_window.hpp"
#include <stdlib.h>
#include <vector>
#include <math.h>
#include <ros/ros.h>
#include <ros/console.h>
#include <vector>
#include <memory.h>
#include <moveit/move_group_interface/move_group.h>
#define GOHOME 0
#define J1ADD 1
#define J1DEC 2
#define J2ADD 3
#define J2DEC 4
#define J3ADD 5
#define J3DEC 6
#define J4ADD 7
#define J4DEC 8
#define J5ADD 9
#define J5DEC 10
#define J6ADD 11
#define J6DEC 12
#define PI 3.14159265358979323846
using namespace std;
/*****************************************************************************
** Namespaces
*****************************************************************************/
namespace zlc_robot {
using namespace Qt;
/*****************************************************************************
** Implementation [MainWindow]
*****************************************************************************/
MainWindow::MainWindow(int argc, char** argv, QWidget *parent)
: QMainWindow(parent)
{
ui.setupUi(this); // Calling this incidentally connects all ui's triggers to on_...() callbacks in this class.
mythread[0] = new zlc_thread;
mythread[1] = new zlc_thread;
mythread[2] = new zlc_thread;
mythread[3] = new zlc_thread;
timer =new QTimer(this);
connect(timer,SIGNAL(timeout()),this,SLOT(timerUpDate()));
ui.lineEdit_step_size->setText("5");
stepsize = 5;
mythread[3]->step_size= stepsize;
}
MainWindow::~MainWindow() {}
/*****************************************************************************
** Implementation [Slots]
*****************************************************************************/
}
//void zlc_robot::MainWindow::on_pushButton_connect_clicked()
//{
// float a; float b; float c;
// QMessageBox::information(this,"hello","add");
// QString str_1 =ui.lineEdit_theta1->text();
// a =str_1.toFloat();
// QString str =ui.lineEdit_theta_2->text();
// b =str.toFloat();
// c = a+b;
// ui.lineEdit_theta_3->setText(QString::number(c));
//}
void zlc_robot::MainWindow::timerUpDate()
{
if(mythread[0]->isjointstate_open == true)
{
for(int i=0;i<6;i++)
{
pos[i]=mythread[3]->pos[i];
pos[i]=pos[i]*180/PI;
}
ui.lineEdit_theta1->setText(QString::number(pos[0], 10, 3));
ui.lineEdit_theta2->setText(QString::number(pos[1], 10, 3));
ui.lineEdit_theta3->setText(QString::number(pos[2], 10, 3));
ui.lineEdit_theta4->setText(QString::number(pos[3], 10, 3));
ui.lineEdit_theta5->setText(QString::number(pos[4], 10, 3));
ui.lineEdit_theta6->setText(QString::number(pos[5], 10, 3));
}
if(mythread[1]->issensorstate_open == true)
{
for(int i=0;i<3;i++)
{
sensor_F[i] = mythread[3]->sensor_F[i];
sensor_T[i] = mythread[3]->sensor_T[i];
}
ui.lineEdit_Fx->setText(QString::number(sensor_F[0], 10, 3));
ui.lineEdit_Fy->setText(QString::number(sensor_F[1], 10, 3));
ui.lineEdit_Fz->setText(QString::number(sensor_F[2], 10, 3));
ui.lineEdit_Tx->setText(QString::number(sensor_T[0], 10, 3));
ui.lineEdit_Ty->setText(QString::number(sensor_T[1], 10, 3));
ui.lineEdit_Tz->setText(QString::number(sensor_T[2], 10, 3));
}
}
void zlc_robot::MainWindow::on_pushButton_open_clicked()
{
mythread[0]->isjointstate_open = true;
mythread[0]->start();
timer->start(10);
}
void zlc_robot::MainWindow::on_pushButton_close_clicked()
{
// mythread[0]->closeThread();
}
void zlc_robot::MainWindow::on_lineEdit_step_size_editingFinished()
{
QString temp_string = ui.lineEdit_step_size->text();
// 将字符串转换成浮点数
double tempfloat = temp_string.toFloat();
// 保存当前的输入值
stepsize = tempfloat;
mythread[3]->step_size= stepsize;
}
void zlc_robot::MainWindow::on_pushButton_J1_add_clicked()
{
// mythread[3]->isjoint_control = true;
mythread[3]->threadmode = J1ADD;
mythread[3]->start();//就是run
}
void zlc_robot::MainWindow::on_pushButton_J1_dec_clicked()
{
// mythread[2]->isjoint_control = true;
mythread[3]->threadmode = J1DEC;
mythread[3]->start();//就是run
}
void zlc_robot::MainWindow::on_pushButton_J2_add_clicked()
{
//mythread[2]->isjoint_control = true;
mythread[3]->threadmode = J2ADD;
mythread[3]->start();//就是run
}
void zlc_robot::MainWindow::on_pushButton_J2_dec_clicked()
{
// mythread[3]->isjoint_control = true;
mythread[3]->threadmode = J2DEC;
mythread[3]->start();//就是run
}
void zlc_robot::MainWindow::on_pushButton_J3_add_clicked()
{
// mythread[2]->isjoint_control = true;
mythread[3]->threadmode = J3ADD;
mythread[3]->start();//就是run
}
void zlc_robot::MainWindow::on_pushButton_J3_dec_clicked()
{
// mythread[2]->isjoint_control = true;
mythread[3]->threadmode = J3DEC;
mythread[3]->start();//就是run
}
void zlc_robot::MainWindow::on_pushButton_J4_add_clicked()
{
// mythread[2]->isjoint_control = true;
mythread[3]->threadmode = J4ADD;
mythread[3]->start();//就是run
}
void zlc_robot::MainWindow::on_pushButton_J4_dec_clicked()
{
//mythread[2]->isjoint_control = true;
mythread[3]->threadmode = J4DEC;
mythread[3]->start();//就是run
}
void zlc_robot::MainWindow::on_pushButton_J5_add_clicked()
{
//mythread[2]->isjoint_control = true;
mythread[3]->threadmode = J5ADD;
mythread[3]->start();//就是run
}
void zlc_robot::MainWindow::on_pushButton_J5_dec_clicked()
{
// mythread[2]->isjoint_control = true;
mythread[3]->threadmode = J5DEC;
mythread[3]->start();//就是run
}
void zlc_robot::MainWindow::on_pushButton_J6_add_clicked()
{
//mythread[2]->isjoint_control = true;
mythread[3]->threadmode = J6ADD;
mythread[3]->start();//就是run
}
void zlc_robot::MainWindow::on_pushButton_J6_dec_clicked()
{
// mythread[2]->isjoint_control = true;
mythread[3]->threadmode = J6DEC;
mythread[3]->start();//就是run
}
//void zlc_robot::MainWindow::on_pushButton_open_sensor_clicked()
//{
// mythread[1]->issensorstate_open = true;
// mythread[1]->start();
//}
void zlc_robot::MainWindow::on_pushButton_open_sensor_clicked()
{
mythread[1]->issensorstate_open = true;
mythread[1]->start();
timer->start(10);
}
这里主要是QThread用的可能有问题，mythread[0]是实现机器人的读取和运动的线程，mythread[1]实现力传感器的读取，mythread[3]是实现ui获取上述线程中数据的线程（弄了很久），ui刷新数据用的是定时器Qtimer,具体见上述线程代码。
3.opto力传感器功能包数据读取
opto功能包链接如下
https://github.com/OptoForce/etherdaq_ros
一些说明如下
# etherdaq_ros
The driver has been tested under Ubuntu 14.04 and ROS indigo.
To build the sources you should use catkin
Compiling
---------
Assuming you are in home directory, and you have root priviliges first you should do the following steps
In a console, type:
* mkdir opto_ws
* cd opto_ws
* mkdir src
* cd src
* catkin_init_workspace
* git clone https://github.com/OptoForce/etherdaq_ros.git
* cd ..
* catkin_make
After successful compiling, you will have two executable files and one library file.
Usage - Demo
------------
First, should check if the EtherDAQ is working by OptoForce Ethernet Discovery Tool. The tool will
show you the address of your EtherDAQ device(s) connected to your network.
To be able to run the ROS demo, you have to modify the following file:
opto_ws/src/opto_ros_driver/ethernet_daq_driver/launch/demo.launch
Replace 192.168.100.12 in the following line args="--address 192.168.100.12 --rate 1000 --filter 4"
to your EtherDAQ's address provided by the Ethernet Discovery Tool.
To run the demo be sure that you are in the home/opto_ws directory!
Type roslaunch optoforce_etherdaq_driver demo.launch
You should see something similar on your console:
[ INFO] [1455791190.094318011]: 2559 Fx:-116.00 Fy:122.00 Fz:141.00 Tx:-183.00 Ty:66.00 Tz:98.00 T:0.06 ms S: 884.97 Hz
[ INFO] [1455791190.095304760]: 2560 Fx:-116.00 Fy:122.00 Fz:141.00 Tx:-183.00 Ty:66.00 Tz:99.00 T:0.98 ms S: 885.17 Hz
[ INFO] [1455791190.096267234]: 2561 Fx:-116.00 Fy:122.00 Fz:141.00 Tx:-184.00 Ty:65.00 Tz:98.00 T:0.96 ms S: 885.40 Hz
[ INFO] [1455791190.097198648]: 2562 Fx:-116.00 Fy:121.00 Fz:141.00 Tx:-183.00 Ty:66.00 Tz:99.00 T:0.93 ms S: 885.67 Hz
[ INFO] [1455791190.097247749]: 2563 Fx:-117.00 Fy:121.00 Fz:141.00 Tx:-183.00 Ty:66.00 Tz:99.00 T:0.05 ms S: 887.15 Hz
[ INFO] [1455791190.099165675]: 2564 Fx:-117.00 Fy:121.00 Fz:141.00 Tx:-184.00 Ty:67.00 Tz:99.00 T:1.91 ms S: 886.08 Hz
[ INFO] [1455791190.099251998]: 2565 Fx:-117.00 Fy:121.00 Fz:141.00 Tx:-184.00 Ty:67.00 Tz:99.00 T:0.09 ms S: 887.50 Hz
If the EtherDAQ does not repsonse, please double check the address in the demo.launch file.
Usage - Real world
------------------
The ROS driver is a simple node providing Force/Torque informations on a ROS topic.
After compiling you can find the node in the /opto_ws/devel/lib/optoforce_etherdaq_driver.
Parameters of the node
----------------------
* --help Shows the help screen and exits.
* --rate (default: 100) (in Hz) The publish speed of the F/T informations. It also sets the EtherDAQ speed to the given value.
* --filter (default: 4) Set the filtering (Valid parameters: 0 = No filter; 1 = 500 Hz; 2 = 150 Hz; 3 = 50 Hz; 4 = 15 Hz; 5 = 5 Hz; 6 = 1.5 Hz)
* --address The IP address of the EtherDAQ device.
* --wrench publish older Wrench message type instead of WrenchStamped
* --frame_id arg (default: "base_link") Frame ID for Wrench data
Example:
"./etherdaq_node --address 192.168.100.12 --rate 500 --filter 3"
* This will start the node with 500 Hz publish rate and with 50 Hz filter.
Topics of the node
------------------
The node subscribes to /etdaq_zero where you can zero the force/torque readings at the current loading level.
* The parameter is std_msgs::Bool, if it's true, then the node zeroes otherwise it unzeroes.
The node publishes to the following topics:
* /diagnostics where you can check the status of the EtherDAQ (speed, last F/T values, system status, address, etc)
* /etherdaq_data the topic where F/T values are published either in Wrench or in WrenchStamped format if the force and torque units are given
* /etherdaq_data_raw topic where F/T values are published either in Wrench or in WrenchStamped format if the force and torque units are not given
The list and short description of source files
----------------------------------------------
* etherdaq_driver.cpp: This is the module which implements the communication between EtherDAQ and ROS.
* etherdaq_node.cpp: This is the module which publish F/T values on ROS topics using etherdaq_driver.cpp
services.
* etherdaq_subscriber.cpp: An example node which subscribes to the topics of etherdaq_node and displays
the actual F/T, speed and elapsed times between two packets. Also this node
does a zero/unzero in every 10 seconds.
连好力传感器，输入如下命令：
roslaunch demo,launch
运行 rqt_graph,节点图如下：
关键是qt如何获取节点发布的数据，这里仿照它的订阅者
在zlc_thread.cpp 中添加如下代码
void sensordatasCallback(const geometry_msgs::WrenchStamped& msg)
{
pthread->sensor_F[0] = msg.wrench.force.x;
pthread->sensor_F[1] = msg.wrench.force.y;
pthread->sensor_F[2] = msg.wrench.force.z;
pthread->sensor_T[0] = msg.wrench.torque.x;
pthread->sensor_T[1] = msg.wrench.torque.y;
pthread->sensor_T[2] = msg.wrench.torque.z;
}
在zlc_thread.cpp run函数中添加
if(issensorstate_open == true)
{
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
// ros::Rate loop_rate(1000); // The loop rate
spinner.start();
ros::Subscriber sub_new = node_handle.subscribe("ethdaq_data", 1000, sensordatasCallback);
while(1)
{
sleep(1); //1ms
// ros::spinOnce();
//loop_rate.sleep();
}
}
在main函数中添加
ros::init(argc, argv, "etherdaq_subscriber");
4.tx90驱动包
参考：https://blog.csdn.net/haishiluoshen/article/details/88044159
https://blog.csdn.net/haishiluoshen/article/details/88045401
https://blog.csdn.net/haishiluoshen/article/details/88044262
驱动和读取的代码间mythread.cpp
rviz上仿照使用staubil_tx90_moveit_config功能包，连实际机器人使用staubil_tx90_moveit功能包，连接实际机器人后示教器上按stop退出后，必须重新开val3应用程序！！！
最后在虚拟仿真下运行如图
版权声明：本文为weixin_42355349原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
THE END