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.getCurrentJoint