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'&");
}