devbox
2023面试高手
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

px4+gazebo无人机仿真，定点起飞，y=x²轨迹飞行_gazebo飞机沿着既定轨迹

作者：2023面试高手 | 2024-05-21 21:44:57

赞

踩

gazebo飞机沿着既定轨迹

目录

一、创建工作空间

1.创建工作空间 catkin_ws

2.编译工作空间 catkin_make

二、offboard位置控制定点起飞

1.准备工作

2.启动

三、offboard位置控制y=x²轨迹飞行

一、创建工作空间

参考ROS学习--第3篇：ROS基础---创建工作空间

1.创建工作空间 catkin_ws

打开终端，创建src文件夹：

mkdir -p ~/catkin_ws/src

进入src文件夹：

cd ~/catkin_ws/src

初始化文件夹：

 catkin_init_workspace

2.编译工作空间 catkin_make

进入catkin_ws文件夹：

cd ~/catkin_ws/

编译：

catkin_make

二、offboard位置控制定点起飞

参考ROS+PX4学习与开发 2.0 ROS与PX4的通信、[PX4]mavros安装+offboard控制过程记录

1.准备工作

进入catking_ws/src文件夹：

cd ~/catkin_ws/src

创建功能包文件夹：

catkin_create_pkg offboard_pkg roscpp std_msgs geometry_msgs mavros_msgs

进入创建好的功能包文件夹的src文件夹内：

cd ~/catkin_ws/src/offboard_pkg/src/

新建一个offboard_node.cpp文件：

touch offboard_node.cpp

然后打开.cpp文件进行编辑，这里用gedit：

gedit offboard_node.cpp

复制进官方示例，然后保存关闭：

  1. /**
  2.  * @file offb_node.cpp
  3.  * @brief Offboard control example node, written with MAVROS version 0.19.x, PX4 Pro Flight
  4.  * Stack and tested in Gazebo SITL
  5.  */
  6.  
  7. #include <ros/ros.h>
  8. #include <geometry_msgs/PoseStamped.h>
  9. #include <mavros_msgs/CommandBool.h>
  10. #include <mavros_msgs/SetMode.h>
  11. #include <mavros_msgs/State.h>
  12.  
  13. mavros_msgs::State current_state;
  14. void state_cb(const mavros_msgs::State::ConstPtr& msg){
  15.     current_state = *msg;
  16. }
  17.  
  18. int main(int argc, char **argv)
  19. {
  20.     ros::init(argc, argv, "offb_node");
  21.     ros::NodeHandle nh;
  22.  
  23.     ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
  24.             ("mavros/state", 10, state_cb);
  25.     ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
  26.             ("mavros/setpoint_position/local", 10);
  27.     ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
  28.             ("mavros/cmd/arming");
  29.     ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
  30.             ("mavros/set_mode");
  31.  
  32.     //the setpoint publishing rate MUST be faster than 2Hz
  33.     ros::Rate rate(20.0);
  34.  
  35.     // wait for FCU connection
  36.     while(ros::ok() && !current_state.connected){
  37.         ros::spinOnce();
  38.         rate.sleep();
  39.     }
  40.  
  41.     geometry_msgs::PoseStamped pose;
  42.     pose.pose.position.x = 0;
  43.     pose.pose.position.y = 0;
  44.     pose.pose.position.z = 2;
  45.  
  46.     //send a few setpoints before starting
  47.     for(int i = 100; ros::ok() && i > 0; --i){
  48.         local_pos_pub.publish(pose);
  49.         ros::spinOnce();
  50.         rate.sleep();
  51.     }
  52.  
  53.     mavros_msgs::SetMode offb_set_mode;
  54.     offb_set_mode.request.custom_mode = "OFFBOARD";
  55.  
  56.     mavros_msgs::CommandBool arm_cmd;
  57.     arm_cmd.request.value = true;
  58.  
  59.     ros::Time last_request = ros::Time::now();
  60.  
  61.     while(ros::ok()){
  62.         if( current_state.mode != "OFFBOARD" &&
  63.             (ros::Time::now() - last_request > ros::Duration(5.0))){
  64.             if( set_mode_client.call(offb_set_mode) &&
  65.                 offb_set_mode.response.mode_sent){
  66.                 ROS_INFO("Offboard enabled");
  67.             }
  68.             last_request = ros::Time::now();
  69.         } else {
  70.             if( !current_state.armed &&
  71.                 (ros::Time::now() - last_request > ros::Duration(5.0))){
  72.                 if( arming_client.call(arm_cmd) &&
  73.                     arm_cmd.response.success){
  74.                     ROS_INFO("Vehicle armed");
  75.                 }
  76.                 last_request = ros::Time::now();
  77.             }
  78.         }
  79.  
  80.         local_pos_pub.publish(pose);
  81.  
  82.         ros::spinOnce();
  83.         rate.sleep();
  84.     }
  85.  
  86.     return 0;
  87. }

打开目录~/catkin_ws/src/offboard_pkg/下的CMakeLists.txt，添加下面的两行：

  1. add_executable(offboard_node src/offboard_node.cpp)
  2. target_link_libraries(offboard_node ${catkin_LIBRARIES})

进入catkin_ws文件夹并编译

  1. cd ~/catkin_ws/
  2.  
  3. catkin_make

打开终端设置环境变量：

  1. echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc
  2. source ~/.bashrc

到此前期准备已经完毕，可以启动。

2.启动

打开1号终端，进入PX4文件夹，打开gazebo：

  1. cd ~/PX4-Autopilot/
  2.  
  3. make px4_sitl gazebo_iris

启动QGC，等待QGC连接成功。

打开2号终端，启动PX4与Mavros之间的连接：

roslaunch mavros px4.launch fcu_url:="udp://:14540@127.0.0.1:14557"

打开3号终端，运行官方示例：

rosrun offboard_pkg offboard_node

稍微等待一会，3号终端如下图所示，正常情况下能看到gazebo中的无人机升高到2m位置。

 如果启动之后，3号终端一直跳offboard enabled但是始终不见Vehicle armed，而且gazebo中无人机也没有反应，可能是PX4版本太新了(13.0)，解决办法是安装1.12.3。

在主文件夹中删除PX4-Autopilot整个文件夹，然后打开终端克隆旧版本：

git clone -b v1.12.3 https://github.com/PX4/PX4-Autopilot.git --recursive

如果下载失败提前结束，报错：

error: RPC failed; curl 56 GnuTLS recv error (-9): A TLS packet with unexpected length was received.
fatal: The remote end hung up unexpectedly
fatal: early EOF
fatal: index-pack failed
Failed during: git fetch origin --force

首先检查网络情况(梯子)，网络检查过没问题还是不行，则在终端输入，然后重新下载：

  1. git config --global http.postBuffer 524288000
  2. git config --global http.maxRequestBuffer 100M
  3. git config --global core.compression 0
  4.  
  5.  
  6. export GIT_TRACE_PACKET=1
  7. export GIT_TRACE=1
  8. export GIT_CURL_VERBOSE=1

如果还是解决不了，参考以下帖子：

【Git】error: RPC failed； curl 56 GnuTLS recv error (-9): A TLS packet with unexpected length was rece

下载PX4固件时网络太慢，经常出现克隆失败

解决git clone 完成后提示'error: RPC failed; curl 56 GnuTLS recv error (-9)'

RPC failed; curl 56 GnuTLS recv error (-54): Error in the pull function.

下载成功之后，编译。

  1. cd PX4-Autopilot
  2. sudo bash ./Tools/setup/ubuntu.sh
  3.  
  4.  
  5. make px4_sitl_default gazebo

如果途中出现问题，参考PX4从放弃到精通（二）：ubuntu18.04配置px4编译环境及mavros环境

重新在.bashrc中添加一下px4源码的路径：

gedit .bashrc

打开后应该可以找到以前添加过得这一段：

source ~/PX4-Autopilot/Tools/setup_gazebo.bash ~/PX4-Autopilot/ ~/PX4-Autopilot/build/px4_sitl_default
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:~/PX4-Autopilot
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:~/PX4-Autopilot/Tools/sitl_gazebo

将其剪切到.bashrc的最下方，保存关闭，然后终端进行刷新

source .bashrc

之后问题解决，重新上面的启动步骤。

如果出现了屏幕突然旋转的情况，解决办法：

xrandr --output eDP-1 --rotate normal

其中eDP-1是显示屏名字，替换为自己的显示屏名字。

三、offboard位置控制y=x²轨迹飞行

参考ROS+PX4无人机室内多点飞行代码

对其.cpp文件修改。

  1. /**
  2.  * @file offb_node.cpp
  3.  * @brief Offboard control example node, written with MAVROS version 0.19.x, PX4 Pro Flight
  4.  * Stack and tested in Gazebo SITL
  5.  */
  6. #include <ros/ros.h>
  7. #include <geometry_msgs/PoseStamped.h>
  8. #include <mavros_msgs/CommandBool.h>
  9. #include <mavros_msgs/SetMode.h>
  10. #include <mavros_msgs/State.h>
  11. #include <nav_msgs/Odometry.h>//里程计信息格式
  12.  
  13. int flag  = 1;
  14.  
  15. mavros_msgs::State current_state;
  16. nav_msgs::Odometry local_pos;
  17.  
  18. void local_pos_cb(const nav_msgs::Odometry::ConstPtr& msg);
  19. void state_cb(const mavros_msgs::State::ConstPtr& msg);
  20.  
  21.  
  22. int main(int argc, char **argv)
  23. {
  24.     ros::init(argc, argv, "offboard_multi_position");
  25.  
  26.     ros::NodeHandle nh;
  27.  
  28.     ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>("mavros/state", 10, state_cb);
  29.  
  30.     ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>("mavros/setpoint_position/local", 10);
  31.     
  32.     ros::Subscriber local_pos_sub = nh.subscribe<nav_msgs::Odometry>("/mavros/local_position/odom", 10, local_pos_cb);
  33.  
  34.     ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");
  35.  
  36.     ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");
  37.  
  38.     //the setpoint publishing rate MUST be faster than 2Hz
  39.     ros::Rate rate(20.0);
  40.  
  41.     // wait for FCU connection
  42.     while(ros::ok() && !current_state.connected)
  43.     {
  44.         ros::spinOnce();
  45.         rate.sleep();
  46.     }
  47.  
  48.     geometry_msgs::PoseStamped pose;
  49.     pose.pose.position.x =   0;
  50.     pose.pose.position.y =   0;
  51.     pose.pose.position.z =   3;  
  52.  
  53.     //send a few setpoints before starting
  54.     for(int i = 100; ros::ok() && i > 0; --i)
  55.     {
  56.         local_pos_pub.publish(pose);
  57.         ros::spinOnce();
  58.         rate.sleep();
  59.     }
  60.  
  61.     mavros_msgs::SetMode offb_set_mode;
  62.     offb_set_mode.request.custom_mode = "OFFBOARD";
  63.  
  64.     mavros_msgs::CommandBool arm_cmd;
  65.     arm_cmd.request.value = true;
  66.  
  67.     ros::Time last_request = ros::Time::now();
  68.  
  69.  	//此处满足一次请求进入offboard模式即可，官方例成循环切入offboard会导致无人机无法使用遥控器控制
  70.     while(ros::ok())
  71.     {
  72.     	//请求进入OFFBOARD模式
  73.         if( current_state.mode != "OFFBOARD" && (ros::Time::now() - last_request > ros::Duration(5.0)))
  74.         {
  75.             if( set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent)
  76.             {
  77.                 ROS_INFO("Offboard enabled");
  78.             }
  79.            	last_request = ros::Time::now();
  80.        	}
  81.         else 
  82. 		{
  83. 			//请求解锁
  84. 			if( !current_state.armed && (ros::Time::now() - last_request > ros::Duration(5.0)))
  85. 			{
  86. 		        if( arming_client.call(arm_cmd) && arm_cmd.response.success)
  87. 		       	{
  88. 		            ROS_INFO("Vehicle armed");
  89. 		        }
  90. 		        	last_request = ros::Time::now();
  91. 			}
  92. 		}
  93. 	    
  94. 	    if(ros::Time::now() - last_request > ros::Duration(5.0))
  95. 	    {
  96. 	    	break;
  97. 	    }	
  98. 	    
  99. 		local_pos_pub.publish(pose);
  100.         ros::spinOnce();
  101.         rate.sleep();
  102.     }   
  103.     
  104.     
  105.     while(ros::ok())
  106.     {
  107.         if((flag == 1)  && (ros::Time::now() - last_request > ros::Duration(5.0)))
  108. 		{ 
  109. 			ROS_INFO("Position_1");
  110.             		pose.pose.position.x =  0;
  111.     			pose.pose.position.y =  0;
  112.     			pose.pose.position.z =  3;                        
  113. 			last_request = ros::Time::now();
  114.             		flag=2;
  115.        		 }
  116.  
  117. 	if((flag ==2) && (ros::Time::now() - last_request > ros::Duration(5.5)))
  118. 		{
  119. 			ROS_INFO("Position_2 ");
  120. 		   	pose.pose.position.x =  pose.pose.position.x + 0.5;
  121. 			pose.pose.position.y =  (pose.pose.position.x)*(pose.pose.position.x);
  122. 			pose.pose.position.z =  3;
  123. 			last_request = ros::Time::now();
  124. 			flag=3;
  125. 		}
  126. 		                   
  127. 	if((flag ==3) && (ros::Time::now() - last_request > ros::Duration(0.5)))
  128. 		{
  129. 		 	ROS_INFO("Position_3 ");
  130. 		 	pose.pose.position.x =  pose.pose.position.x + 0.5;
  131. 			pose.pose.position.y =  (pose.pose.position.x)*(pose.pose.position.x);
  132.             		pose.pose.position.z =  3;   
  133. 			last_request = ros::Time::now();
  134. 			flag=4;
  135. 		}
  136.  
  137. 	if((flag ==4) && (ros::Time::now() - last_request > ros::Duration(0.5)))
  138. 		{
  139. 		 	ROS_INFO("Position_4 ");
  140. 		 	pose.pose.position.x =  pose.pose.position.x + 0.5;
  141. 			pose.pose.position.y =  (pose.pose.position.x)*(pose.pose.position.x);
  142. 		 	pose.pose.position.z =  3;
  143. 		 	last_request = ros::Time::now();
  144. 			flag=5;
  145. 		}
  146.  
  147. 	if((flag ==5) && (ros::Time::now() - last_request > ros::Duration(0.5)))
  148. 		{
  149.            		ROS_INFO("Position_5 ");
  150.             		pose.pose.position.x =  pose.pose.position.x + 0.5;
  151. 			pose.pose.position.y =  (pose.pose.position.x)*(pose.pose.position.x);
  152.             		pose.pose.position.z =  3;  		
  153.             		last_request = ros::Time::now();
  154. 			flag=6;
  155.         	}
  156. 	if((flag ==6) && (ros::Time::now() - last_request > ros::Duration(0.5)))
  157. 		{
  158.            		ROS_INFO("Position_6 ");
  159.             		pose.pose.position.x =  pose.pose.position.x + 0.5;
  160. 			pose.pose.position.y =  (pose.pose.position.x)*(pose.pose.position.x);
  161.             		pose.pose.position.z =  3;  		
  162.             		last_request = ros::Time::now();
  163. 			flag=7;
  164.         	}
  165. 	if((flag ==7) && (ros::Time::now() - last_request > ros::Duration(0.5)))
  166. 		{
  167.            		ROS_INFO("Position_7 ");
  168.             		pose.pose.position.x =  pose.pose.position.x + 0.5;
  169. 			pose.pose.position.y =  (pose.pose.position.x)*(pose.pose.position.x);
  170.             		pose.pose.position.z =  3;  		
  171.             		last_request = ros::Time::now();
  172. 			flag=8;
  173.         	}
  174. 	if((flag ==8) && (ros::Time::now() - last_request > ros::Duration(0.5)))
  175. 		{
  176.            		ROS_INFO("Position_8 ");
  177.             		pose.pose.position.x =  pose.pose.position.x + 0.5;
  178. 			pose.pose.position.y =  (pose.pose.position.x)*(pose.pose.position.x);
  179.             		pose.pose.position.z =  3;  		
  180.             		last_request = ros::Time::now();
  181. 			flag=9;
  182.         	}
  183. 	if((flag ==9) && (ros::Time::now() - last_request > ros::Duration(0.5)))
  184. 		{
  185.            		ROS_INFO("Position_9 ");
  186.             		pose.pose.position.x =  pose.pose.position.x + 0.5;
  187. 			pose.pose.position.y =  (pose.pose.position.x)*(pose.pose.position.x);
  188.             		pose.pose.position.z =  3;  		
  189.             		last_request = ros::Time::now();
  190. 			flag=10;
  191.         	}
  192. 	if((flag ==10) && (ros::Time::now() - last_request > ros::Duration(0.5)))
  193. 		{
  194.            		ROS_INFO("Position_10 ");
  195.             		pose.pose.position.x =  pose.pose.position.x + 0.5;
  196. 			pose.pose.position.y =  (pose.pose.position.x)*(pose.pose.position.x);
  197.             		pose.pose.position.z =  3;  		
  198.             		last_request = ros::Time::now();
  199. 			flag=11;
  200.         	}
  201. 	if((flag ==11) && (ros::Time::now() - last_request > ros::Duration(0.5)))
  202. 		{
  203.            		ROS_INFO("Position_11 ");
  204.             		pose.pose.position.x =  pose.pose.position.x + 0.5;
  205. 			pose.pose.position.y =  (pose.pose.position.x)*(pose.pose.position.x);
  206.             		pose.pose.position.z =  3;  		
  207.             		last_request = ros::Time::now();
  208. 			flag=12;
  209.         	}
  210. 	if((flag ==12) && (ros::Time::now() - last_request > ros::Duration(0.5)))
  211. 		{
  212.            		ROS_INFO("Position_12 ");
  213.             		pose.pose.position.x =  pose.pose.position.x + 0.5;
  214. 			pose.pose.position.y =  (pose.pose.position.x)*(pose.pose.position.x);
  215.             		pose.pose.position.z =  3;  		
  216.             		last_request = ros::Time::now();
  217. 			flag=13;
  218.         	}
  219. 	if((flag ==13) && (ros::Time::now() - last_request > ros::Duration(0.5)))
  220. 		{
  221.            		ROS_INFO("Position_13 ");
  222.             		pose.pose.position.x =  pose.pose.position.x + 0.5;
  223. 			pose.pose.position.y =  (pose.pose.position.x)*(pose.pose.position.x);
  224.             		pose.pose.position.z =  3;  		
  225.             		last_request = ros::Time::now();
  226. 			flag=14;
  227.         	}
  228.         if((flag ==14) && (ros::Time::now() - last_request > ros::Duration(8.0)))
  229.         	{
  230.         		break;
  231.         	}
  232.  
  233.         local_pos_pub.publish(pose);
  234.         ros::spinOnce();
  235.         rate.sleep();
  236.     }
  237.     
  238.     ROS_INFO("AUTO.LAND");
  239.     offb_set_mode.request.custom_mode = "AUTO.LAND";
  240.     set_mode_client.call(offb_set_mode);
  241.     return 0;
  242. }
  243.  
  244. void state_cb(const mavros_msgs::State::ConstPtr& msg)
  245. {
  246.     current_state = *msg;
  247. }
  248.  
  249. void local_pos_cb(const nav_msgs::Odometry::ConstPtr& msg)
  250. {
  251.     local_pos = *msg;
  252. }

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/2023面试高手/article/detail/604775
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号