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

通过mavros+gazebo+px4实现offboard模式螺旋线（胎教级别）_vscode px4

作者：知新_RL | 2024-06-17 04:31:52

赞

踩

vscode px4

1.简介

本文将介绍通过mavros+gazebo+px4实现offboard模式,无人机螺旋上升的超简单流程。很多小伙伴在刚接触offoard的时候都会遇到跑不通例程的问题，看博客时一知半解，即使博主给出代码也不知道代码怎么运行怎样build文件。

 2.运行环境

2.1.版本要求

px4安装版本：https://dev.px4.io/v1.11_noredirect/en/setup/dev_env_linux_ubuntu.html

ubuntu ：20.04

ros : noetic

mavros

qgroundcountrol

gazebo版本信息： Gazebo multi-robot simulator, version 11.13.0 Copyright (C) 2012 Open Source Robotics Foundation. Released under the Apache 2 License. http://gazebosim.org

安装过程不再赘述，按照官方教程，问问师兄，使用正确上网方式，基本都很好装。

3.实现流程

3.1启动 px4和gazebo节点

找到PX4的安装路径，make一下（我的在根目录）：

  1. cd Firmware #进入自己的目录下
  2. make px4_sitl gazebo_iris

有报错不影响，出现如下输出基本没问题，有问题可以提出在评论区。

  1. INFO  [mavlink] MAVLink only on localhost (set param MAV_{i}_BROADCAST = 1 to enable network)
  2. INFO  [px4] Startup script returned successfully
  3. pxh> INFO  [mavlink] partner IP: 127.0.0.1
  4. INFO  [commander] Ready for takeoff!

3.2 启动ros核心

新建一个端口启动ros核心

roscore   #不要关闭！！

3.3启动QGroundControl.AppImage

我测试过4.0.8是没有问题的，需要注意连接方式，一般可以自动连接。如果不能的话按下图自行建立连接。

3.4建立mavros与px4的通信

这段代码说实话，我也不是很理解，大概意思是确定了就是主机的通信端口以及软环中px4的udp端口。

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

4. 建立功能包

4.1 创建工作空间

  1. mkdir -p catkin_ws/src       #(必须得有 src)
  2. cd catkin_ws
  3. catkin_make

进入 catkin_ws 启动 vscode

  1. cd catkin_ws
  2. code .
vscode 中创建功能包并编译 ros：

在vscode的资料管理器中选中选定 src 右击 ---> create catkin package。

在弹出的对话框输入：

offboard_sin

回车后，再输入依赖：

roscpp std_msgs geometry_msgs mavros_msgs

建立正确的文件夹后，新建文件offboard_node.cpp的位置如图所示： 

（如果这步不会可以参考http://www.autolabor.com.cn/book/ROSTutorials/chapter1/14-ros-ji-cheng-kai-fa-huan-jing-da-jian/142-an-zhuang-vscode.html以及对应的课程，该文档对应的课程在b站有公开课，从文档可以找到对应连接）

建立cpp文件后,复制如下代码到文件中，用快捷键 ctrl + shift + B 调用编译，选择:catkin_make:build：

（（根据官方offboard代码示例改写，链接：Redirecting to latest version of document (main)））

  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 <geometry_msgs/Vector3.h>
  10. #include <mavros_msgs/CommandBool.h>
  11. #include <mavros_msgs/SetMode.h>
  12. #include <mavros_msgs/State.h>
  13.  
  14. #define PI acos(-1)
  15.  
  16. mavros_msgs::State current_state;
  17. geometry_msgs::PoseStamped current_position;
  18. void state_cb(const mavros_msgs::State::ConstPtr& msg){
  19.     current_state = *msg;
  20. }
  21. void getpointfdb(const geometry_msgs::PoseStamped::ConstPtr& msg){
  22.     ROS_INFO("x: [%f]", msg->pose.position.x);
  23.     ROS_INFO("y: [%f]", msg->pose.position.y);
  24.     ROS_INFO("z: [%f]", msg->pose.position.z);
  25.     current_position = *msg;
  26. }
  27.  
  28. int main(int argc, char **argv)
  29. {
  30.     ros::init(argc, argv, "offb_node");
  31.     ros::NodeHandle nh;
  32.  
  33.     ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
  34.             ("mavros/state", 10, state_cb);
  35.             
  36.     ros::Subscriber get_point = nh.subscribe<geometry_msgs::PoseStamped>
  37.             ("mavros/local_position/pose", 10, getpointfdb);
  38.             
  39.     ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
  40.             ("mavros/setpoint_position/local", 10);
  41.     ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
  42.             ("mavros/cmd/arming");
  43.     ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
  44.             ("mavros/set_mode");
  45.  
  46.     //the setpoint publishing rate MUST be faster than 2Hz
  47.     ros::Rate rate(20.0f);
  48.  
  49.     // wait for FCU connection
  50.     while(ros::ok() && !current_state.connected){
  51.         ros::spinOnce();
  52.         rate.sleep();
  53.     }
  54.  
  55.     geometry_msgs::PoseStamped pose;
  56.     pose.pose.position.x = 0;
  57.     pose.pose.position.y = 0;
  58.     pose.pose.position.z = 2;
  59.     
  60.     
  61.     
  62.  
  63.     //send a few setpoints before starting
  64.     for(int i = 100; ros::ok() && i > 0; --i){
  65.         local_pos_pub.publish(pose);
  66.         ros::spinOnce();
  67.         rate.sleep();
  68.     }
  69.  
  70.     mavros_msgs::SetMode offb_set_mode;
  71.     offb_set_mode.request.custom_mode = "OFFBOARD";//将飞行模式设置为 "OFFBOARD"。
  72.  
  73.     mavros_msgs::CommandBool arm_cmd;//用于解锁或锁定无人机。
  74.     arm_cmd.request.value = true;//请求解锁无人机。
  75.  
  76.     ros::Time last_request = ros::Time::now();//将当前的ROS时间赋值给变量 last_request
  77.  
  78.     while(ros::ok()){
  79.         if( current_state.mode != "OFFBOARD" &&
  80.             (ros::Time::now() - last_request > ros::Duration(5.0f))){
  81.             if( set_mode_client.call(offb_set_mode) &&
  82.                 offb_set_mode.response.mode_sent){
  83.                 ROS_INFO("Offboard enabled");//確保飛行模式為offboard
  84.             }
  85.             last_request = ros::Time::now();
  86.         } else {
  87.             if( !current_state.armed &&
  88.                 (ros::Time::now() - last_request > ros::Duration(5.0f))){
  89.                 if( arming_client.call(arm_cmd) &&
  90.                     arm_cmd.response.success){
  91.                     ROS_INFO("Vehicle armed");//確保飛行解鎖
  92.                 }
  93.                 last_request = ros::Time::now();
  94.             }
  95.         }
  96.          
  97.         if((abs(current_position.pose.position.x-pose.pose.position.x)<0.5f)
  98.             &&(abs(current_position.pose.position.y-pose.pose.position.y)<0.5f)
  99.             &&(abs(current_position.pose.position.y-pose.pose.position.y)<0.5f))
  100.         {
  101.             pose.pose.position.x = 20*sin(pose.pose.position.z);
  102.             pose.pose.position.y = 20*cos(pose.pose.position.z);
  103.             pose.pose.position.z += 0.001;
  104.         }
  105.  
  106.         local_pos_pub.publish(pose);
  107.  
  108.         ros::spinOnce();
  109.         rate.sleep();
  110.     }
  111.  
  112.     return 0;
  113. }
  114.

修改src/offboard_sin/CMakeLists.txt路经下的CMakeLists.txt文件，在最后加入下面两行(一定是文末)：

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

编译代码

快捷键 ctrl + shift + B 调用编译，选择:catkin_make:build

编译完成会提示如下类似内容：

  1. [build] [1/2  50% :: 1.971] Building CXX object CMakeFiles/offboard_node.dir/src/offboard_node.cpp.o
  2. [build] [2/2 100% :: 2.095] Linking CXX executable devel/lib/offboard_sin/offboard_node
  3. [driver] Build completed: 00:00:02.112
  4. [build] Build finished with exit code 0

结束啦！然后就是rosrun运行节点，发布无人机的位置话题。

5.例程实现

编译完成后，运行节点的终端里，一定记得在终端source一下。具体路径每个人不一样，可以参考之前那个连接里的公开课教程。如下是我的工作空间路径和执行的命令，不要全文复制，执行的命令是：source ./devel/setup.bash

~/catkin_ws$ source ./devel/setup.bash

把钱前面的东西都配置好以后，就可以按照我图里的终端运行啦！

运行顺序可以自行探索，注意终端命令和文件夹的匹配。我给出一个比较好理解的顺序：

不要铁头复制，$后面才是命令!前面是路径！！！

  1. #第一个终端0
  2. yqc@yqc-virtual-machine:~$ roscore #开启ros核心
  3. #新建终端1
  4. yqc@yqc-virtual-machine:~/Firmware$ make px4_sitl gazebo_iris #makepx4软环和gazebo仿真环境
  5. #新建终端2
  6. yqc@yqc-virtual-machine:~/catkin_ws$ rosrun offboard_sin offboard_node  #启动位置发布节点 
  7. #新建终端3
  8. yqc@yqc-virtual-machine:~$ roslaunch mavros px4.launch fcu_url:="udp://:14540@127.0.0.1:14557"  #建立通信

 运行结果图：

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

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

  

闽ICP备14008679号