0. 前言
在仿真环境中经常需要使用键盘控制无人机去拍摄一些图像和点云，相比编写代码控制无人机，使用键盘控制更方便一些。这篇博客中，我们一起来使用 C++ 编写代码来接收键盘输入以实现无人机的位置控制。
一般无人机遥控器使用的是速度控制，可以实现无人机的转向。这里之所以不使用是因为使用键盘来改变速度不便于控制，使用位置控制也难以实现无人机的转向。通常情况下，无人机的转向是通过调整其姿态（如航向角）来实现的，而不是通过改变位置坐标。姿态控制涉及到更多的控制参数和算法，以确保无人机能够稳定地旋转。在实际应用中，无人机的转向通常通过速度控制或者姿态控制来实现，而不是仅仅通过位置控制。速度控制可以调整无人机的旋转速度，而姿态控制可以确保无人机在旋转过程中保持稳定。因此，要实现无人机的转向，通常需要结合速度控制和姿态控制，以实现预期的转向动作。具体的实现方式会根据无人机的硬件和软件平台而有所不同。
下面是这篇博客将要实现的键盘操作说明书：
| 按键 | 控制作用 | 操控 |
|---|---|---|
| w/W | 将目标位置的高度增加0.1 | 上升 |
| s/S | 如果目标位置的高度大于0.1，则将目标位置的高度减少0.1 | 下降 |
| l/L | 将目标位置的y坐标减少0.05 | 右移 |
| i/I | 将目标位置的x坐标增加0.05 | 前移 |
| j/J | 将目标位置的y坐标增加0.05 | 左移 |
| k/K | 将目标位置的x坐标减少0.05 | 后移 |
| q/Q | 退出程序 |
1. 创建功能包
catkin_create_pkg keyboard roscpp mavros_msgs laser_geometry pcl_conversions pcl_ros sensor_msgs tf
2. 修改 CMakeLists.txt
find_package(Eigen3)
find_package(PCL 1.10 REQUIRED)
catkin_package(
INCLUDE_DIRS include
LIBRARIES keyboard
CATKIN_DEPENDS laser_geometry mavros_msgs pcl_conversions pcl_ros roscpp sensor_msgs tf
# DEPENDS system_lib
)
include_directories(
include
${catkin_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}
)
add_executable(control_node src/control_node.cpp)
add_executable(keyboard_pub src/keyboard_pub.cpp)
target_link_libraries(control_node
${catkin_LIBRARIES}
${PCL_LIBRARIES}
)
target_link_libraries(control_node
${catkin_LIBRARIES}
${PCL_LIBRARIES}
)
3. 编写代码
3.1 编写 control_node.h
#include <iostream>
#include <string>
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <laser_geometry/laser_geometry.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/Imu.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/RCIn.h>
#include <mavros_msgs/WaypointList.h>
#include <rosgraph_msgs/Clock.h>
#include <mavros_msgs/WaypointReached.h>
using namespace std;
class PX4_Control
{
public:
PX4_Control();
// void IMU_Data();
void State_Callback(const mavros_msgs::State::ConstPtr &msg);
void Local_Pose_Callback(const geometry_msgs::PoseStamped::ConstPtr &msg);
void WayPoint_Callback(const mavros_msgs::WaypointList::ConstPtr &msg);
void WaypointReached_Pose_Callback(const mavros_msgs::WaypointReached::ConstPtr &msg);
ros::NodeHandle px4_control_node;
mavros_msgs::State Current_State;
geometry_msgs::PoseStamped Local_Pose;
mavros_msgs::WaypointList WaypointList;
mavros_msgs::WaypointReached WaypointReached;
ros::Subscriber State_Sub;
ros::Subscriber Local_Pose_Sub;
ros::Subscriber WayPoint_Sub;
ros::Subscriber WaypointReached_Sub;
ros::Publisher Target_Pos_Pub;
ros::ServiceClient Arming_Client;
ros::ServiceClient Set_Mode_Client;
};
PX4_Control::PX4_Control()
{
State_Sub = px4_control_node.subscribe("mavros/state", 10, &PX4_Control::State_Callback, this);
Local_Pose_Sub = px4_control_node.subscribe("mavros/local_position/pose", 10, &PX4_Control::Local_Pose_Callback, this);
WayPoint_Sub = px4_control_node.subscribe("/mavros/mission/waypoints", 10, &PX4_Control::WayPoint_Callback, this);
WaypointReached_Sub = px4_control_node.subscribe("/mavros/mission/reached", 10, &PX4_Control::WaypointReached_Pose_Callback, this);
Target_Pos_Pub = px4_control_node.advertise<geometry_msgs::PoseStamped>("mavros/setpoint_position/local", 10);
Arming_Client = px4_control_node.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");
Set_Mode_Client = px4_control_node.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");
}
void PX4_Control::State_Callback(const mavros_msgs::State::ConstPtr &msg)
{
Current_State = *msg;
}
void PX4_Control::Local_Pose_Callback(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
Local_Pose = *msg;
}
void PX4_Control::WayPoint_Callback(const mavros_msgs::WaypointList::ConstPtr &msg)
{
WaypointList = *msg;
}
void PX4_Control::WaypointReached_Pose_Callback(const mavros_msgs::WaypointReached::ConstPtr &msg)
{
WaypointReached= *msg;
}
3.2 编写 keyboard_pub.cpp
#include <istream>
#include <iostream>
#include <termio.h>
#include <cstring>
#include <ros/ros.h>
#include <std_msgs/Char.h>
// 获取键盘输入字符
char scanKeyboard()
{
char input;
struct termios new_settings;
struct termios stored_settings;
tcgetattr(0, &stored_settings);
new_settings = stored_settings;
new_settings.c_lflag &= (~ICANON);
new_settings.c_cc[VTIME] = 0;
tcgetattr(0, &stored_settings);
new_settings.c_cc[VMIN] = 1;
tcsetattr(0, TCSANOW, &new_settings);
input = std::cin.get();
tcsetattr(0, TCSANOW, &stored_settings);
return input;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "keyboard_publisher");
ros::NodeHandle nh;
ros::Publisher keyboard_pub = nh.advertise<std_msgs::Char>("keyboard_input", 1);
while (ros::ok())
{
// 获取键盘输入
char input = scanKeyboard();
// 创建键盘消息
std_msgs::Char msg;
msg.data = input;
// 发布键盘消息
keyboard_pub.publish(msg);
// 循环等待回调函数执行
ros::spinOnce();
if (input == 'q')
break;
}
return 0;
}
3.3 编写 control_node.cpp
#include "control_node.h"
#include <istream>
#include <termio.h>
#include <cstring>
#include <std_msgs/Char.h>
int test_cnt = 0;
// 获取键盘输入字符
char scanKeyboard()
{
char input;
struct termios new_settings;
struct termios stored_settings;
tcgetattr(0, &stored_settings);
new_settings = stored_settings;
new_settings.c_lflag &= (~ICANON);
new_settings.c_cc[VTIME] = 0;
tcgetattr(0, &stored_settings);
new_settings.c_cc[VMIN] = 1;
tcsetattr(0, TCSANOW, &new_settings);
input = cin.get();
tcsetattr(0, TCSANOW, &stored_settings);
return input;
}
// 发布目标位置
void publishTargetPosition(const geometry_msgs::PoseStamped &pose, ros::Publisher &targetPosPub, ros::Rate &loopRate)
{
targetPosPub.publish(pose);
// ros::spinOnce();
// loopRate.sleep();
}
// 检查是否满足起飞条件
bool checkTakeoffConditions(const PX4_Control &px4_control)
{
return px4_control.Current_State.armed;
}
void keyboardCallback(const std_msgs::Char::ConstPtr &msg, geometry_msgs::PoseStamped &pose, const PX4_Control &px4_control, float target_altitude, ros::Publisher &targetPosPub, ros::Rate &loopRate, bool &control_pose_flag, bool &exit_flag)
{
if (checkTakeoffConditions(px4_control))
{
if (control_pose_flag && px4_control.Local_Pose.pose.position.z < target_altitude * 0.9)
{
// 起飞后保持初始位置
pose.pose.position.x = 0;
pose.pose.position.y = 0;
pose.pose.position.z = target_altitude;
}
else
{
control_pose_flag = false;
// 根据键盘消息执行相应操作
char input = msg->data;
switch (input)
{
case 'w':
case 'W':
// 执行向上的操作
pose.pose.position.z += 0.1;
std::cout << pose.pose.position.z << std::endl;
break;
case 's':
case 'S':
// 执行向下的操作
if (pose.pose.position.z > 0.1)
{
pose.pose.position.z -= 0.1;
}
break;
case 'l':
case 'L':
// 执行向右平移的操作
pose.pose.position.y -= 0.05;
break;
case 'i':
case 'I':
// 执行向前平移的操作
pose.pose.position.x += 0.05;
break;
case 'j':
case 'J':
// 执行向左平移的操作
pose.pose.position.y += 0.05;
break;
case 'k':
case 'K':
// 执行向后平移的操作
pose.pose.position.x -= 0.05;
break;
case 'q':
case 'Q':
// 执行退出的操作
exit_flag = true;
break;
default:
break;
}
}
}
}
int main(int argc, char **argv)
{
float target_altitude = 2;
float radius = 200;
ros::init(argc, argv, "px4_lidar");
PX4_Control px4_control;
ros::Rate loop_rate(100);
while (ros::ok() && px4_control.Current_State.connected)
{
ros::spinOnce();
loop_rate.sleep();
}
geometry_msgs::PoseStamped pose;
pose.pose.position.x = 0;
pose.pose.position.y = 0;
pose.pose.position.z = target_altitude;
// 发送一些初始的目标位置
for (int i = 100; ros::ok() && i > 0; --i)
{
publishTargetPosition(pose, px4_control.Target_Pos_Pub, loop_rate);
}
mavros_msgs::SetMode offb_set_mode;
offb_set_mode.request.custom_mode = "OFFBOARD";
mavros_msgs::CommandBool arm_cmd;
arm_cmd.request.value = true;
ros::Time last_request = ros::Time::now();
while (ros::ok() && !px4_control.Current_State.armed)
{
if (px4_control.Current_State.mode != "OFFBOARD" && (ros::Time::now() - last_request > ros::Duration(5.0)))
{
if (px4_control.Set_Mode_Client.call(offb_set_mode) && offb_set_mode.response.mode_sent)
{
ROS_INFO("Offboard enabled");
}
last_request = ros::Time::now();
}
else if (!px4_control.Current_State.armed && (ros::Time::now() - last_request > ros::Duration(5.0)))
{
if (px4_control.Arming_Client.call(arm_cmd) && arm_cmd.response.success)
{
ROS_INFO("Vehicle armed");
}
last_request = ros::Time::now();
}
px4_control.Target_Pos_Pub.publish(pose);
ros::spinOnce();
loop_rate.sleep();
}
bool control_pose_flag = true;
bool exit_flag = false;
ros::NodeHandle keyboard_sub_node;
ros::Subscriber keyboard_sub = keyboard_sub_node.subscribe<std_msgs::Char>("keyboard_input", 1,
[&](const std_msgs::Char::ConstPtr& msg) {
keyboardCallback(msg, pose, px4_control, target_altitude, px4_control.Target_Pos_Pub, loop_rate, control_pose_flag, exit_flag);
});
while (ros::ok())
{
if (exit_flag)
{
break;
}
ros::spinOnce();
loop_rate.sleep();
px4_control.Target_Pos_Pub.publish(pose);
}
return 0;
}
3.4 编译代码
catkin_make
4. 依次执行
roslaunch px4 mavros_posix_sitl_new.launch
rosrun keyboard control_node
rosrun keyboard keyboard_pub
实现效果如下图所示，在 keyboard_pub 终端窗口中输入字符，以进行控制：
评论（0）
您还未登录，请登录后发表或查看评论