ros qt gui机械臂ur robot正逆运动学界面

widget.cpp

#include "widget.h"
#include <QString>
#include <QDebug>
#include <QMessageBox>
#include <std_msgs/Int8.h>
#include <fstream>//文件读写
#include<vector>//提供向量头文件
#include<algorithm>//算法头文件，提供迭代器
#include<iomanip>//输出精度控制需要
#include <unistd.h>

Widget::Widget(int argc, char **argv, QWidget *parent) :
    QWidget(parent),
    ui(new Ui::Widget)
{
    ui->setupUi(this);    
    
    /*获取ip*/
    char name[256];
	gethostname(name, sizeof(name));
	struct hostent* host = gethostbyname(name);
	char ipStr[32];
	const char* ret = inet_ntop(host->h_addrtype, host->h_addr_list[0], ipStr, sizeof(ipStr));
	if (NULL==ret) {
		std::cout << "hostname transform to ip failed"<<endl;
	}
    else
    {
        std::cout<<ipStr<<endl;
        ui->edit_ip->setText(QString(ipStr));
    }//获取ip结束

    init_ros(argc, argv);//初始化ros
    /*获取ros master uri */
    XmlRpc::XmlRpcValue args, result, payload;
    args[0] = ros::this_node::getName();

    if (!ros::master::execute("getUri", args, result, payload, true)) {
        std::cout << "Failed!" << std::endl;
    }
    std::cout << "\n----------Master URI----------" << std::endl;
    std::cout << std::string(payload) << std::endl;
    QString uri=QString::fromStdString(std::string(payload));
    ui->edit_uri->setText(QString(uri));//获取ros master uri结束
    
    QObject::connect(ui->pushButton_start, SIGNAL(clicked()), this, SLOT(slot_btn_start()));
    QObject::connect(ui->pushButton_quit, SIGNAL(clicked()), this, SLOT(slot_btn_quit()));

    m_timer = new QTimer( this );
    QObject::connect( m_timer, SIGNAL(timeout()), this, SLOT(slot_timer()));
}

Widget::~Widget()
{
    delete m_timer;
    delete ui;
}

void Widget::init_ros(int argc, char **argv)
{
    ros::init(argc, argv, "ros_cmake");
    ros::NodeHandle private_nh("~");
    test_pub_ = private_nh.advertise<std_msgs::Int8>("test", 1000);
}

void Widget::on_btn_FKrun_clicked()
{
    std::cout<<"正运动学规划\n"<<endl;
    float joint1=ui->fk_joint_1->text().toFloat();
    float joint2=ui->fk_joint_2->text().toFloat();
    float joint3=ui->fk_joint_3->text().toFloat();
    float joint4=ui->fk_joint_4->text().toFloat();
    float joint5=ui->fk_joint_5->text().toFloat();
    float joint6=ui->fk_joint_6->text().toFloat();
    std::string path="/home/cbc/catkin_ws/src/ur4_simulation/src/";
    std::string inputPath= path+"fk_input.txt";
    std::ofstream fout(inputPath);
    fout<<joint1<<" "<<joint2<<" "<<joint3<<" "<<joint4<<" "<<joint5<<" "<<joint6;
    fout.close();
    system("gnome-terminal -x bash -c 'source ~/catkin_ws/devel/setup.bash; roslaunch ur4_description_moveit_config demo.launch'&");
    // sleep(5);//延时1s
    system("gnome-terminal -x bash -c 'source ~/catkin_ws/devel/setup.bash; rosrun ur4_simulation fk.py'&");
    // sleep(3);//延时1s
    // std::string path="/home/cbc/catkin_ws/src/ur4_simulation/src/";
    std::string outputpath=path+"fk_output.txt";
    std::vector<double> V;
    std::vector<double>::iterator it;
    std::ifstream data(outputpath);
    double d;
    while (data>>d)
    {
        V.push_back(d);//数据存入
    }
    data.close();
    int i=0;
    double pose[10];
    for(it=V.begin();it!=V.end();it++)
    {
        std::cout<<V[i]<<" ";
        pose[i]=*it;
        i++;
    }
    std::cout<<endl;
     ui->pose_x->setText(QString::number(pose[0], 10, 4));
     ui->pose_y->setText(QString::number(pose[1], 10, 4));
     ui->pose_z->setText(QString::number(pose[2], 10, 4));
}

void Widget::on_btn_IKrun_clicked()
{
    std::cout<<"逆运动学规划\n"<<endl;
    float input_x=ui->input_x->text().toFloat();
    float input_y=ui->input_y->text().toFloat();
    float input_z=ui->input_z->text().toFloat();
    float input_ox=ui->input_ox->text().toFloat();
    float input_oy=ui->input_oy->text().toFloat();
    float input_oz=ui->input_oz->text().toFloat();
    float input_ow=ui->input_ow->text().toFloat();
    std::string path="/home/cbc/catkin_ws/src/ur4_simulation/src/";
    std::string inputPath= path+"ik_input.txt";
    std::ofstream fout(inputPath);
    fout<<input_x<<" "<<input_y<<" "<<input_z<<" "<<input_ox<<" "<<input_oy<<" "<<input_oz<<" "<<input_ow;
    fout.close();
    system("gnome-terminal -x bash -c 'source ~/catkin_ws/devel/setup.bash; roslaunch ur4_description_moveit_config demo.launch'&");
    // sleep(2);//延时1s
    system("gnome-terminal -x bash -c 'source ~/catkin_ws/devel/setup.bash; rosrun ur4_simulation ik.py'&");
    // sleep(1);//延时1s
    // std::string path="/home/cbc/catkin_ws/src/ur4_simulation/src/";
    std::string outputpath=path+"ik_output.txt";
    std::vector<double> V;
    std::vector<double>::iterator it;
    std::ifstream data(outputpath);
    double d;
    while (data>>d)
    {
        V.push_back(d);//数据存入
    }
    data.close();
    int i=0;
    double pose[10];
    for(it=V.begin();it!=V.end();it++)
    {
        std::cout<<V[i]<<" ";
        pose[i]=*it;
        i++;
    }
    std::cout<<endl;
     ui->ik_joint_1->setText(QString::number(pose[0], 10, 4));
     ui->ik_joint_2->setText(QString::number(pose[1], 10, 4));
     ui->ik_joint_3->setText(QString::number(pose[2], 10, 4));
     ui->ik_joint_4->setText(QString::number(pose[3], 10, 4));
     ui->ik_joint_5->setText(QString::number(pose[4], 10, 4));
     ui->ik_joint_6->setText(QString::number(pose[5], 10, 4));
}

void Widget::closeEvent(QCloseEvent *ev)
{
    // int result = QMessageBox::question(this, QString("Tips"), QString("是否退出？"));
    // if(QMessageBox::Yes == result)
        ev->accept();
    // else
    //     ev->ignore();
}

void Widget::on_btn_show_robot_clicked()//机械臂展示按钮
{
    QImage img_robot;
    if(img_robot.load("/home/cbc/tool/ros_ur_gui/robot.png"))//加载该路径下的机械臂图片 需自行更改路径
    {
        ui->label_show_robot->setPixmap(QPixmap::fromImage(img_robot));
    }
}

void Widget::on_btn_rviz_clicked()
{
    system("gnome-terminal -x bash -c 'source ~/catkin_ws/devel/setup.bash; roslaunch ur4_description display.launch'&");//需自行更改catkin_ws路径，下同
}

void Widget::on_btn_boot_moveit_clicked()
{
    system("gnome-terminal -x bash -c 'source ~/catkin_ws/devel/setup.bash; roslaunch ur4_gazebo ur4_bringup_moveit.launch'&");
}

void Widget::on_btn_obstacle_clicked()
{
    system("gnome-terminal -x bash -c 'source ~/catkin_ws/devel/setup.bash; rosrun ur4_simulation obstacle-avoiding.py'&");
}