pvqf6mep3
/
Air_Ground_CEC
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

add twist

Browse Source
pull/10/head
大耳刮子 3 years ago
parent 01c3158d4e
commit 545537255b
8 changed files with 179 additions and 287 deletions
Show Stats Download Patch File Download Diff File

25
src/include/qnode.hpp
Unescape View File

@ -4,25 +4,46 @@
#ifndef Q_MOC_RUN
#include <ros/ros.h>
#endif
#include <QImage>
#include <geometry_msgs/Twist.h>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <std_msgs/String.h>
#include <ros/network.h>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <geometry_msgs/Twist.h>
#include <QThread>
class QNode : public QThread{
Q_OBJECT
public:
QNode(int argc, char **argv);
virtual ~QNode();
bool init();
void SubAndPubTopic();
void KeyboardMove(char key);
//void myCallback_img(const sensor_msgs::ImageConstPtr& msg);//camera callback function
//QImage image;
//Q_SIGNALS:
//void loggingCamera();//发出设置相机图片信号
private:
int init_argc;
char **init_argv;
ros::NodeHandle n;
ros::Subscriber cmdVel_sub;
ros::Publisher cmd_pub;
//ros::Publisher chatter_publisher;
//QStringList logging_model;
//image_transport::Subscriber image_sub;
//cv::Mat img;
};
#endif

58
src/include/roskeyboardteleopnode.h
Unescape View File

@ -1,58 +0,0 @@
#ifndef ROSKEYBOARDTELEOPNODE_H
#define ROSKEYBOARDTELEOPNODE_H
#include <termios.h>
#include <signal.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/poll.h>
#include <boost/thread/thread.hpp>
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#define KEYCODE_W 0x77
#define KEYCODE_A 0x61
#define KEYCODE_S 0x73
#define KEYCODE_D 0x64
#define KEYCODE_A_CAP 0x41
#define KEYCODE_D_CAP 0x44
#define KEYCODE_S_CAP 0x53
#define KEYCODE_W_CAP 0x57
class RosKeyboardTeleopNode
{
private:
double walk_vel_;
double run_vel_;
double yaw_rate_;
double yaw_rate_run_;
geometry_msgs::Twist cmdvel_;
ros::NodeHandle n_;
ros::Publisher pub_;
public:
RosKeyboardTeleopNode();
~RosKeyboardTeleopNode() { }
void keyboardLoop();
void stopRobot()
{
cmdvel_.linear.x = 0.0;
cmdvel_.angular.z = 0.0;
pub_.publish(cmdvel_);
}
};
extern int kfd;
extern struct termios cooked, raw;
extern bool done;
extern RosKeyboardTeleopNode* tbk;
#endif // ROSKEYBOARDTELEOPNODE_H

17
src/main/main.cpp
Unescape View File

@ -1,4 +1,4 @@
#include "../include/roskeyboardteleopnode.h"
#include "../ui/mainwindow.hpp"
#include <QApplication>
@ -11,25 +11,12 @@ int main(int argc, char** argv)
{
ros::init(argc,argv,"tbk", ros::init_options::AnonymousName | ros::init_options::NoSigintHandler);
QApplication app(argc, argv);
MainWindow CEC(0,0,0);
CEC.show();
return app.exec();
/*
RosKeyboardTeleopNode tbk;
boost::thread t = boost::thread(boost::bind(&RosKeyboardTeleopNode::keyboardLoop, &tbk));
ros::spin();
t.interrupt();
t.join();
tbk.stopRobot();
tcsetattr(kfd, TCSANOW, &cooked);
*/
return(0);
}

25
src/main/mainwindow.cpp
Unescape View File

@ -1,12 +1,14 @@
#include "../ui/mainwindow.hpp"
MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
QMainWindow(parent), qnode(argc, argv),
ui(new Ui::MainWindow)
{
qnode.init();
ui->setupUi(this);
connections();
}
@ -41,7 +43,6 @@ void MainWindow::slot_keyboard_control(){
qnode.KeyboardMove(key);
std::cout<<key<<std::endl;
}
@ -56,4 +57,24 @@ void MainWindow::connections(){
ui->pushButton_D->setShortcut(Qt::Key_D);
connect(ui->pushButton_D, SIGNAL(clicked()), this, SLOT(slot_keyboard_control()));
connect(ui->pushButton_shift, SIGNAL(clicked()), this, SLOT(slot_keyboard_control()));
//
//QObject::connect(&qnode, SIGNAL(loggingCamera()), this, SLOT(slot_updateCamera()));
}
/*
void MainWindow::slot_dispalyCamera(const QImage &image){
qimage_mutex.lock();
qimage = image.copy();
ui->UGV_Vedio->setPixmap(QPixmap::fromImage(qimage));
ui->UGV_Vedio->resize(ui->UGV_Vedio->pixmap()->size());
qimage_mutex.unlock();
}
void MainWindow::slot_updateCamera()
{
slot_dispalyCamera(qnode.image);
}
*/

45
src/main/qnode.cpp
Unescape View File

@ -1,11 +1,11 @@
#include "../include/qnode.hpp"
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <ros/network.h>
#include "sensor_msgs/image_encodings.h"
QNode::QNode(int argc, char **argv) : init_argc(argc), init_argv(argv){
cmd_pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 1);
}
@ -18,15 +18,16 @@ QNode::~QNode() {
}
bool QNode::init() {
ros::init(init_argc, init_argv, "cyrobot_monitor",
ros::init_options::AnonymousName);
if (!ros::master::check()) {
return false;
}
ros::start(); // explicitly needed since our nodehandle is going out of
// scope.
start();
return true;
ros::init(init_argc,init_argv,"Air_Ground_CEC",
ros::init_options::AnonymousName | ros::init_options::NoSigintHandler);
SubAndPubTopic();
return true;
}
void QNode::SubAndPubTopic(){
ros::NodeHandle n;
cmd_pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 1);
}
void QNode::KeyboardMove(char key){
@ -45,3 +46,21 @@ void QNode::KeyboardMove(char key){
cmd_pub.publish(twist);
ros::spinOnce();
}
/*
void QNode::myCallback_img(const sensor_msgs::ImageConstPtr &msg)
{
try
{
cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::RGB8);
img = cv_ptr->image;
image = QImage(img.data,img.cols,img.rows,img.step[0],QImage::Format_RGB888);//change to QImage format
ROS_INFO("I'm setting picture in mul_t callback function!");
Q_EMIT loggingCamera();
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
}
}
*/

138
src/main/roskeyboardteleopnode.cpp
Unescape View File

@ -1,138 +0,0 @@
#include "../include/roskeyboardteleopnode.h"
int kfd = 0;
struct termios cooked, raw;
bool done;
RosKeyboardTeleopNode* tbk;
RosKeyboardTeleopNode::RosKeyboardTeleopNode()
{
pub_ = n_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
ros::NodeHandle n_private("~");
n_private.param("walk_vel", walk_vel_, 0.5);
n_private.param("run_vel", run_vel_, 1.0);
n_private.param("yaw_rate", yaw_rate_, 1.0);
n_private.param("yaw_rate_run", yaw_rate_run_, 1.5);
}
void RosKeyboardTeleopNode::keyboardLoop()
{
char c;
double max_tv = walk_vel_;
double max_rv = yaw_rate_;
bool dirty = false;
int speed = 0;
int turn = 0;
// get the console in raw mode
tcgetattr(kfd, &cooked);
memcpy(&raw, &cooked, sizeof(struct termios));
raw.c_lflag &=~ (ICANON | ECHO);
raw.c_cc[VEOL] = 1;
raw.c_cc[VEOF] = 2;
tcsetattr(kfd, TCSANOW, &raw);
puts("Reading from keyboard");
puts("Use WASD keys to control the robot");
puts("Press Shift to move faster");
struct pollfd ufd;
ufd.fd = kfd;
ufd.events = POLLIN;
for(;;)
{
boost::this_thread::interruption_point();
// get the next event from the keyboard
int num;
if ((num = poll(&ufd, 1, 250)) < 0)
{
perror("poll():");
return;
}
else if(num > 0)
{
if(read(kfd, &c, 1) < 0)
{
perror("read():");
return;
}
}
else
{
if (dirty == true)
{
stopRobot();
dirty = false;
}
continue;
}
switch(c)
{
case KEYCODE_W:
max_tv = walk_vel_;
speed = 1;
turn = 0;
dirty = true;
break;
case KEYCODE_S:
max_tv = walk_vel_;
speed = -1;
turn = 0;
dirty = true;
break;
case KEYCODE_A:
max_rv = yaw_rate_;
speed = 0;
turn = 1;
dirty = true;
break;
case KEYCODE_D:
max_rv = yaw_rate_;
speed = 0;
turn = -1;
dirty = true;
break;
case KEYCODE_W_CAP:
max_tv = run_vel_;
speed = 1;
turn = 0;
dirty = true;
break;
case KEYCODE_S_CAP:
max_tv = run_vel_;
speed = -1;
turn = 0;
dirty = true;
break;
case KEYCODE_A_CAP:
max_rv = yaw_rate_run_;
speed = 0;
turn = 1;
dirty = true;
break;
case KEYCODE_D_CAP:
max_rv = yaw_rate_run_;
speed = 0;
turn = -1;
dirty = true;
break;
default:
max_tv = walk_vel_;
max_rv = yaw_rate_;
speed = 0;
turn = 0;
dirty = false;
}
cmdvel_.linear.x = speed * max_tv;
cmdvel_.angular.z = turn * max_rv;
pub_.publish(cmdvel_);
}
}

7
src/ui/mainwindow.hpp
Unescape View File

@ -2,6 +2,9 @@
#define MAINWINDOW_H
#include <QMainWindow>
#include <QImage>
#include <QMutex>
#include "ui_mainwindow.h"
#include "../include/qnode.hpp"
@ -19,11 +22,15 @@ public:
public slots:
void slot_keyboard_control();
//void slot_updateCamera();
//void slot_dispalyCamera(const QImage& image);
private:
Ui::MainWindow *ui;
void connections();
QNode qnode;
//QImage qimage;
//mutable QMutex qimage_mutex;
};

151
src/ui/mainwindow.ui
Unescape View File

@ -14,29 +14,16 @@
<string>MainWindow</string>
</property>
<widget class="QWidget" name="centralwidget">
<widget class="QWidget" name="">
<widget class="QWidget" name="layoutWidget">
<property name="geometry">
<rect>
<x>410</x>
<y>260</y>
<x>1160</x>
<y>240</y>
<width>314</width>
<height>164</height>
</rect>
</property>
<layout class="QGridLayout" name="gridLayout">
<item row="0" column="0">
<spacer name="horizontalSpacer">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item row="0" column="1">
<widget class="QPushButton" name="pushButton_W">
<property name="minimumSize">
@ -61,21 +48,8 @@
</property>
</widget>
</item>
<item row="0" column="2">
<spacer name="horizontalSpacer_2">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item row="1" column="0">
<widget class="QPushButton" name="pushButton_A">
<item row="1" column="2">
<widget class="QPushButton" name="pushButton_D">
<property name="minimumSize">
<size>
<width>100</width>
@ -94,12 +68,12 @@
</font>
</property>
<property name="text">
<string>左</string>
<string>右</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QPushButton" name="pushButton_shift">
<item row="1" column="0">
<widget class="QPushButton" name="pushButton_A">
<property name="minimumSize">
<size>
<width>100</width>
@ -118,12 +92,12 @@
</font>
</property>
<property name="text">
<string>加速</string>
<string>左</string>
</property>
</widget>
</item>
<item row="1" column="2">
<widget class="QPushButton" name="pushButton_D">
<item row="2" column="1">
<widget class="QPushButton" name="pushButton_S">
<property name="minimumSize">
<size>
<width>100</width>
@ -142,25 +116,12 @@
</font>
</property>
<property name="text">
<string>右</string>
<string>后</string>
</property>
</widget>
</item>
<item row="2" column="0">
<spacer name="horizontalSpacer_3">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item row="2" column="1">
<widget class="QPushButton" name="pushButton_S">
<item row="1" column="1">
<widget class="QPushButton" name="pushButton_shift">
<property name="minimumSize">
<size>
<width>100</width>
@ -179,23 +140,95 @@
</font>
</property>
<property name="text">
<string>后</string>
<string>加速</string>
</property>
</widget>
</item>
</layout>
</widget>
<widget class="QLabel" name="UGV_Vedio">
<property name="geometry">
<rect>
<x>610</x>
<y>60</y>
<width>321</width>
<height>211</height>
</rect>
</property>
<property name="text">
<string>TextLabel</string>
</property>
</widget>
<widget class="QWidget" name="">
<property name="geometry">
<rect>
<x>60</x>
<y>30</y>
<width>502</width>
<height>654</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout">
<item>
<widget class="QLabel" name="UAV_Vedio">
<property name="minimumSize">
<size>
<width>500</width>
<height>300</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>500</width>
<height>300</height>
</size>
</property>
<property name="mouseTracking">
<bool>false</bool>
</property>
<property name="styleSheet">
<string notr="true"/>
</property>
<property name="text">
<string>UAV_VEDIO</string>
</property>
<property name="scaledContents">
<bool>false</bool>
</property>
</widget>
</item>
<item row="2" column="2">
<spacer name="horizontalSpacer_4">
<item>
<spacer name="verticalSpacer">
<property name="orientation">
<enum>Qt::Horizontal</enum>
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
<width>20</width>
<height>40</height>
</size>
</property>
</spacer>
</item>
<item>
<widget class="QLabel" name="UAV_Path">
<property name="minimumSize">
<size>
<width>500</width>
<height>300</height>
</size>
</property>
<property name="maximumSize">
<size>
<width>500</width>
<height>300</height>
</size>
</property>
<property name="text">
<string>PATH</string>
</property>
</widget>
</item>
</layout>
</widget>
</widget>

Write Preview
Loading…
Cancel
Save