You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
240 lines
6.5 KiB
240 lines
6.5 KiB
#include "../include/Air_Ground_CEC/qnode.hpp"
|
|
|
|
#include "sensor_msgs/image_encodings.h"
|
|
#include <QtConcurrent/QtConcurrent>
|
|
|
|
QNode::QNode(int argc, char **argv) : init_argc(argc), init_argv(argv){
|
|
|
|
|
|
}
|
|
|
|
|
|
QNode::~QNode() {
|
|
if (ros::isStarted()) {
|
|
ros::shutdown(); // explicitly needed since we use ros::start();
|
|
ros::waitForShutdown();
|
|
}
|
|
wait();
|
|
}
|
|
|
|
bool QNode::init() {
|
|
QSettings topic_setting("ros_controller", "settings");
|
|
odom_topic = topic_setting.value("topic/topic_odom", "odom").toString();
|
|
batteryState_topic =
|
|
topic_setting.value("topic/topic_power", "battery_state").toString();
|
|
qRegisterMetaType<sensor_msgs::BatteryState>("sensor_msgs::BatteryState");
|
|
ros::init(init_argc,init_argv,"Air_Ground_CEC",
|
|
ros::init_options::AnonymousName | ros::init_options::NoSigintHandler);
|
|
QtConcurrent::run([=](){
|
|
SubAndPubTopic();
|
|
});
|
|
|
|
|
|
return true;
|
|
}
|
|
|
|
void QNode::SubAndPubTopic(){
|
|
ros::NodeHandle n;
|
|
|
|
//创建速度话题的订阅者
|
|
cmdVel_sub = n.subscribe<nav_msgs::Odometry>(odom_topic.toStdString(), 10,
|
|
&QNode::speedCallback, this);
|
|
|
|
//power_sub = n.subscribe(batteryState_topic.toStdString(), 1000,
|
|
// &QNode::powerCallback, this);
|
|
cmd_pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 1);
|
|
battery_sub = n.subscribe(batteryState_topic.toStdString(), 1000,
|
|
&QNode::batteryCallback, this);
|
|
|
|
image_transport::ImageTransport it(n);
|
|
image_sub = it.subscribe("camera/rgb/image_raw", 100, &QNode::imageCallback, this);
|
|
|
|
//image_sub0 = n.subscribe("camera/rgb/image_raw",100,&QNode::imageCallback,this)
|
|
|
|
//chatter_publisher = n.advertise<std_msgs::String>("chatter1", 1000);
|
|
//chatter_subscriber = n.subscribe("chatter",1000,&QNode::myCallback,this);
|
|
ros::spin();
|
|
}
|
|
|
|
void QNode::batteryCallback(const sensor_msgs::BatteryState& message) {
|
|
emit batteryState(message);
|
|
}
|
|
|
|
void QNode::KeyboardMove(char key, float speed_linear, float speed_trun){
|
|
std::map<char, std::vector<float>> moveBindings {
|
|
// {'key', {x, y, z, th}}
|
|
{'7', {1, 1, 0, 1}}, {'8', {1, 0, 0, 0}}, {'9', {1, -1, 0, -1}},
|
|
{'4', {0, 0, 0, 1}}, {'5', {0, 0, 0, 0}}, {'6', {0, 0, 0, -1}},
|
|
{'1', {-1, 1, 0, -1}}, {'2', {-1, 0, 0, 0}}, {'3', {-1, -1, 0, 1}}
|
|
};
|
|
|
|
float x = moveBindings[key][0];
|
|
float y = moveBindings[key][1];
|
|
float z = moveBindings[key][2];
|
|
float th = moveBindings[key][3];
|
|
|
|
//计算线速度和角速度
|
|
float speed = speed_linear;
|
|
float turn = speed_trun;
|
|
|
|
|
|
|
|
geometry_msgs::Twist twist;
|
|
twist.linear.x = x * speed;
|
|
twist.linear.y = y * speed;
|
|
twist.linear.z = z * speed;
|
|
|
|
twist.angular.x = 0;
|
|
twist.angular.y = 0;
|
|
twist.angular.z = th * turn;
|
|
|
|
cmd_pub.publish(twist);
|
|
|
|
//ros::spinOnce();
|
|
}
|
|
|
|
void QNode::AutoMove(float speed_linear, float speed_turn,
|
|
float target_linear, float target_turn){
|
|
|
|
// wrong command
|
|
if (speed_linear==0 && speed_turn ==0){
|
|
std::cout<<"wrong command"<<endl;
|
|
return;
|
|
}
|
|
|
|
|
|
//How fast will we update the robot's movement?
|
|
double rate = 1;
|
|
//Set the equivalent ROS rate variable
|
|
ros::Rate loopRate(rate);
|
|
|
|
std::map<char, std::vector<float>> Directions {
|
|
{'w',{1, 0, 0, 0}}, {'x',{-1, 0, 0, 0}}, {'s',{0, 0, 0, 0}},
|
|
{'a',{0, 0, 0, 1}}, {'d',{0, 0, 0, -1}}
|
|
};
|
|
char key = 's';
|
|
if(target_linear < 0){
|
|
key = 'x';
|
|
}
|
|
else if (target_linear > 0){
|
|
key = 'w';
|
|
}
|
|
else if (target_turn > 0){
|
|
key = 'a';
|
|
}
|
|
else if (target_turn < 0){
|
|
key = 'd';
|
|
}
|
|
|
|
float x = Directions[key][0];
|
|
float y = Directions[key][1];
|
|
float z = Directions[key][2];
|
|
float th = Directions[key][3];
|
|
|
|
|
|
|
|
float total_times = 0;
|
|
if (target_linear == 0){
|
|
total_times = fabs(target_turn)/speed_turn;
|
|
}
|
|
else{
|
|
total_times = fabs(target_linear)/speed_linear;
|
|
}
|
|
|
|
std::cout<< total_times<<std::endl;
|
|
|
|
int ticks;
|
|
geometry_msgs::Twist twist;
|
|
|
|
|
|
twist.linear.x = speed_linear * x;
|
|
twist.linear.y = speed_linear * y;
|
|
twist.linear.z = speed_linear * z;
|
|
|
|
twist.angular.x = 0;
|
|
twist.angular.y = 0;
|
|
twist.angular.z = speed_turn * th;
|
|
|
|
ticks = int(total_times);
|
|
|
|
for(int i = 0; i < ticks; i++)
|
|
{
|
|
cmd_pub.publish(twist); // 将刚才设置的指令发送给机器人
|
|
loopRate.sleep();
|
|
}
|
|
|
|
ros::spinOnce();
|
|
}
|
|
|
|
|
|
//速度回调函数
|
|
void QNode::speedCallback(const nav_msgs::Odometry::ConstPtr& msg)
|
|
{
|
|
emit speed_x(msg->twist.twist.linear.x);
|
|
emit speed_th(msg->twist.twist.angular.z);
|
|
|
|
}
|
|
|
|
|
|
void QNode::imageCallback(const sensor_msgs::ImageConstPtr &msg)
|
|
{
|
|
try
|
|
{
|
|
cv_bridge::CvImageConstPtr cv_ptr =
|
|
cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::RGB8);
|
|
cv::Mat img = cv_ptr->image;
|
|
QImage 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!");
|
|
//image.save("/home/jackyma/test.jpg");
|
|
emit show_image(image);
|
|
}
|
|
catch (cv_bridge::Exception& e)
|
|
{
|
|
ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
|
|
}
|
|
|
|
}
|
|
|
|
|
|
QImage QNode::Mat2QImage(cv::Mat const& src) {
|
|
QImage dest(src.cols, src.rows, QImage::Format_ARGB32);
|
|
|
|
const float scale = 255.0;
|
|
|
|
if (src.depth() == CV_8U) {
|
|
if (src.channels() == 1) {
|
|
for (int i = 0; i < src.rows; ++i) {
|
|
for (int j = 0; j < src.cols; ++j) {
|
|
int level = src.at<quint8>(i, j);
|
|
dest.setPixel(j, i, qRgb(level, level, level));
|
|
}
|
|
}
|
|
} else if (src.channels() == 3) {
|
|
for (int i = 0; i < src.rows; ++i) {
|
|
for (int j = 0; j < src.cols; ++j) {
|
|
cv::Vec3b bgr = src.at<cv::Vec3b>(i, j);
|
|
dest.setPixel(j, i, qRgb(bgr[2], bgr[1], bgr[0]));
|
|
}
|
|
}
|
|
}
|
|
} else if (src.depth() == CV_32F) {
|
|
if (src.channels() == 1) {
|
|
for (int i = 0; i < src.rows; ++i) {
|
|
for (int j = 0; j < src.cols; ++j) {
|
|
int level = scale * src.at<float>(i, j);
|
|
dest.setPixel(j, i, qRgb(level, level, level));
|
|
}
|
|
}
|
|
} else if (src.channels() == 3) {
|
|
for (int i = 0; i < src.rows; ++i) {
|
|
for (int j = 0; j < src.cols; ++j) {
|
|
cv::Vec3f bgr = scale * src.at<cv::Vec3f>(i, j);
|
|
dest.setPixel(j, i, qRgb(bgr[2], bgr[1], bgr[0]));
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return dest;
|
|
}
|