[ROS项目]无人机仿真（位置控制） |
您所在的位置：网站首页 › 虚拟仿真无人机怎么起飞 › [ROS项目]无人机仿真（位置控制） |
[ROS项目]无人机仿真（位置控制）
|
无人机仿真（位置控制）
本次项目所用的仿真环境是开源的Rotors功能包，初次了解无人机的控制，也是摸索小白，希望可以起到抛砖引玉的作用。
1. 安装Rotors软件功能包
首先需要建立ROS软件包，并下载对应的Rotors的软件功能包： $ mkdir -p /ROS/rotors_ws/src $ cd ROS/rotors_ws/src $ catkin_init_workspace $ git clone https://github.com/xmy0916/rotors之后编译git下来的代码即可： $ cd .. $ catkin_make $ source devel/setup.sh 2. 运行Rotors的Firefly机器人的launch文件Firefly机器人是一个六轴的机器人，带有三个摄像头，从rviz的topic中可以看到有深度、RGB双目摄像头。出于个人喜好，可以对launch文件中的world进行修改，选择不同的运行物理环境，我喜欢沙都没有。所以，用了basic.world来对无人机进行仿真。订阅后的摄像头图像会发现看到了上面螺旋桨的旋转，所以我在mav_with_vi_sensor.gazebo文件中对摄像头的位置进行了微调，为以后的建图以及图像识别的容错率提升打好基础。
在调整好摄像头的角度后，遇到了其他插件无法转换至bask_link上的问题，需要将坐标系换成world坐标系，来完成转换。 3. 控制Firefly首先在控制交互上我选用的是通过扫描键盘按下的键值，然后进行对应的控制。所以识别被按下键盘值成为首要解决。 #include #include #include #include #include #include #include #include #include static struct termios initial_settings, new_settings; static int peek_character = -1; void init_keyboard(void); void close_keyboard(void); int kbhit(void); int readch(void); void init_keyboard() { tcgetattr(0,&initial_settings); new_settings = initial_settings; new_settings.c_lflag |= ICANON; new_settings.c_lflag |= ECHO; new_settings.c_lflag |= ISIG; new_settings.c_cc[VMIN] = 1; new_settings.c_cc[VTIME] = 0; tcsetattr(0, TCSANOW, &new_settings); } void close_keyboard() { tcsetattr(0, TCSANOW, &initial_settings); } int kbhit() { unsigned char ch; int nread; if (peek_character != -1) return 1; new_settings.c_cc[VMIN]=0; tcsetattr(0, TCSANOW, &new_settings); nread = read(0,&ch,1); new_settings.c_cc[VMIN]=1; tcsetattr(0, TCSANOW, &new_settings); if(nread == 1) { peek_character = ch; return 1; } return 0; } int readch() { char ch; if(peek_character != -1) { ch = peek_character; peek_character = -1; return ch; } read(0,&ch,1); return ch; } int main(int argc, char** argv) { init_keyboard(); ros::init(argc, argv, "waypoint_publisher"); ros::NodeHandle nh("//firefly"); ros::Publisher trajectory_pub = nh.advertise(mav_msgs::default_topics::COMMAND_TRAJECTORY, 10); ROS_INFO("Started waypoint_publisher."); const float DEG_2_RAD = M_PI / 180.0; float pos_x = 0,pos_y = 0,pos_z = 1; float yaw_deg = 0; int isChange = 1; while(ros::ok()) { if(kbhit()) { int key = readch(); switch(key) { case 56: pos_z += 0.5; isChange = 1; printf("%d\n",key); break; case 50://2 pos_z -= 0.5; isChange = 1; printf("%d\n",key); break; case 97://a pos_y += 0.5; isChange = 1; printf("%d\n",key); break; case 100://d pos_y -= 0.5; isChange = 1; printf("%d\n",key); break; case 119://w pos_x += 0.5; isChange = 1; printf("%d\n",key); break; case 120://x pos_x -= 0.5; isChange = 1; printf("%d\n",key); break; default: printf("%d\n",key); break; } } if(isChange == 1) { trajectory_msgs::MultiDOFJointTrajectory trajectory_msg; trajectory_msg.header.stamp = ros::Time::now(); Eigen::Vector3d desired_position(pos_x, pos_y,pos_z); double desired_yaw = yaw_deg * DEG_2_RAD; mav_msgs::msgMultiDofJointTrajectoryFromPositionYaw(desired_position, desired_yaw, &trajectory_msg); ROS_INFO("Publishing waypoint on namespace %s: [%f, %f, %f].", nh.getNamespace().c_str(), desired_position.x(), desired_position.y(), desired_position.z()); trajectory_pub.publish(trajectory_msg); isChange = 0; } ros::spinOnce(); } ros::shutdown(); return 0; }程序中首先定义了三个函数：void init_keyboard(void)、void close_keyboard(void)和int kbhit(void),第一个是用来对键盘的初始化、第二个函数是tcsetattr是用于设置终端参数的函数，而第三个函数的存在原因是，Windows系统下可以使用_kbhit()函数来获取键盘事件，来判断是否有按键被按下，而Unix/Linux中，没有提供kbhit()函数，所以我们需要自己来实现kbhit()程序。 在键盘控制中我们设定了6个键，分别对应前进、后退、向左、向右、上升和下降，没有加入旋转选项。在分析发布数据格式时候进行讨论。对控制话题发布的格式是ros::Publisher trajectory_pub = nh.advertise (mav_msgs::default_topics::COMMAND_TRAJECTORY, 10);，所以根据该代码可以发现话题是：COMMAND_TRAJECTORY且数据格式是trajectory_msgs::MultiDOFJointRrajectory通过rosmsg show来查询该数据格式的具体内容 $ rosmsg show trajectory_msgs/MultiDOFJointTrajectory std_msgs/Header header uint32 seq time stamp string frame_id string[] joint_names trajectory_msgs/MultiDOFJointTrajectoryPoint[] points geometry_msgs/Transform[] transforms geometry_msgs/Vector3 translation float64 x float64 y float64 z geometry_msgs/Quaternion rotation float64 x float64 y float64 z float64 w geometry_msgs/Twist[] velocities geometry_msgs/Vector3 linear float64 x float64 y float64 z geometry_msgs/Vector3 angular float64 x float64 y float64 z geometry_msgs/Twist[] accelerations geometry_msgs/Vector3 linear float64 x float64 y float64 z geometry_msgs/Vector3 angular float64 x float64 y float64 z duration time_from_start根据上面的数据格式的结果显示进行分析，其消息格式下主要涵盖了两个类型的消息：string[] joint_names和trajectory_msgs/MultiDOFJointTrajectoryPoint[] points而且后者又涵盖了三个子消息类型：geometry_msgs/Transform[] transforms、geometry_msgs/Twist[] velocities和geometry_msgs/Twist[] accelerations主要对无人机进行控制。 在对位置进行控制的时候，首先对x,y,z三个坐标进行数据获取，之后定义好yaw角度，也就是俯仰角（对无人机不进行角度的控制，只是单纯的位置控制）。 到这里可能会有一个疑问，说我发布三个坐标值的数据，那个无人机怎么就可以对坐标进行跟踪，或者移动带该坐标点了？我这里想说明的是，在运行控制无人机的结点之前，需要在Gazebo中建立无人机模型以及控制方案，在之前机械臂控制中就需要ros_control的控制方案的选择，那么接下来对运行无人机模型的launch文件进行描述。 前面一些对变量的定义和目标文件位置的定义进行跳过，第一个运行的文件是gazebo_ros软件包下运行设定好的world文件的launch可执行launch文件，目的是将环境带入一些现实生活中的物理模型。接着第二个执行文件就是通过spawn_mav.launch对无人机模型进行导入。接着，也就是最重要的一部，lee_position_controller_node可执行文件就是对无人机的6个电机进行速度的控制，完成位置控制。接下来的三个结点就是发布一些自身无人机的数据，而rviz就是将无人机模型及其自身的摄像头进行订阅和显示。 最后放上一个演示效果图和演示视频，某人说放上EXO照片会长粉也不知道考不靠谱。
可是看到这高糊的图片，注定与爱豆EXO无缘了。 最后就放上正常的控制仿真视频的图片了： |
【本文地址】
今日新闻 |
推荐新闻 |