【rotors】多旋翼无人机仿真（一）——搭建rotors仿真环境
【rotors】多旋翼无人机仿真（二）——设置飞行轨迹
【rotors】多旋翼无人机仿真（三）——SE3控制
【rotors】多旋翼无人机仿真（四）——参数补偿和PID控制
【rotors】多旋翼无人机仿真（五）——多无人机仿真

本贴内容参考两位博主的内容：
月照银海似蛟
Reed Liao

1、前言

在上一节中，我们安装了rotors，并分析了启动程序的launch文件，其中有两个节点程序比较重要：lee_position_controller_node 、 hovering_example，这一节我们来看一下hovering_example节点程序的内容。

2、源码解析

hovering_example节点程序文件位置在~/UAV_rotors/src/rotors_simulator/rotors_gazebo/src/hovering_example.cpp，源码及其注释如下：

/** Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland* Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland* Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland* Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland* Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland** Licensed under the Apache License, Version 2.0 (the "License");* you may not use this file except in compliance with the License.* You may obtain a copy of the License at**     http://www.apache.org/licenses/LICENSE-2.0* Unless required by applicable law or agreed to in writing, software* distributed under the License is distributed on an "AS IS" BASIS,* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.* See the License for the specific language governing permissions and* limitations under the License.*/#include <thread>
#include <chrono>#include <Eigen/Core>
#include <mav_msgs/conversions.h>
#include <mav_msgs/default_topics.h>
#include <ros/ros.h>
#include <std_srvs/Empty.h>
#include <trajectory_msgs/MultiDOFJointTrajectory.h>int main(int argc, char** argv) {ros::init(argc, argv, "hovering_example");ros::NodeHandle nh;// Create a private node handle for accessing node parameters.// nh_private的命名空间是相对路径，可参考https://blog.csdn.net/mynameisJW/article/details/115816641ros::NodeHandle nh_private("~");// 创建一个发布者，用于发送轨迹消息ros::Publisher trajectory_pub =nh.advertise<trajectory_msgs::MultiDOFJointTrajectory>(mav_msgs::default_topics::COMMAND_TRAJECTORY, 10);ROS_INFO("Started hovering example.");std_srvs::Empty srv;bool unpaused = ros::service::call("/gazebo/unpause_physics", srv);unsigned int i = 0;// Trying to unpause Gazebo for 10 seconds.// 等待Gazebo启动10秒while (i <= 10 && !unpaused) {ROS_INFO("Wait for 1 second before trying to unpause Gazebo again.");std::this_thread::sleep_for(std::chrono::seconds(1));unpaused = ros::service::call("/gazebo/unpause_physics", srv);++i;}if (!unpaused) {ROS_FATAL("Could not wake up Gazebo.");return -1;} else {ROS_INFO("Unpaused the Gazebo simulation.");}// Wait for 5 seconds to let the Gazebo GUI show up.ros::Duration(5.0).sleep();// 定义轨迹消息的变量trajectory_msgs::MultiDOFJointTrajectory trajectory_msg;trajectory_msg.header.stamp = ros::Time::now();// Default desired position and yaw.// 定义期望位置和偏航角Eigen::Vector3d desired_position(0.0, 0.0, 1.0);double desired_yaw = 0.0;// Overwrite defaults if set as node parameters.nh_private.param("x", desired_position.x(), desired_position.x());nh_private.param("y", desired_position.y(), desired_position.y());nh_private.param("z", desired_position.z(), desired_position.z());nh_private.param("yaw", desired_yaw, desired_yaw);// 将desired_position desired_yaw转换数据类型为trajectory_msgmav_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);ros::spinOnce();ros::shutdown();return 0;
}

可见hovering_example节点程序是一个发布轨迹消息的简单demo，我们用户方只需要创建一个轨迹话题的发布句柄，然后publish期望的轨迹点即可，在无人机路径规划中，首先我们规划出路径点然后将路径点插值出一条光滑连续的轨迹点，之后不断publish这些轨迹点即可实现无人机的移动。

3、编写一个发布圆形轨迹的节点程序

通过简单的修改hovering_example内容，我们就可以写一个简单的发布圆形轨迹的节点程序，在~/UAV_rotors/src/rotors_simulator/rotors_gazebo/src文件夹中创建run_circle_locus.cpp文件，内容如下：

//  run_circle_locus.cpp
// 飞一个圆形轨迹#include <thread>
#include <chrono>
#include <cmath>
#include <Eigen/Core>
#include <mav_msgs/conversions.h>
#include <mav_msgs/default_topics.h>
#include <ros/ros.h>
#include <std_srvs/Empty.h>
#include <trajectory_msgs/MultiDOFJointTrajectory.h>int main(int argc, char** argv) {ros::init(argc, argv, "circle_locus_example");ros::NodeHandle nh;// Create a private node handle for accessing node parameters.ros::NodeHandle nh_private("~");ros::Publisher trajectory_pub =nh.advertise<trajectory_msgs::MultiDOFJointTrajectory>(mav_msgs::default_topics::COMMAND_TRAJECTORY, 10);ROS_INFO("Started circle locus example.");std_srvs::Empty srv;bool unpaused = ros::service::call("/gazebo/unpause_physics", srv);unsigned int i = 0;// Trying to unpause Gazebo for 10 seconds.while (i <= 10 && !unpaused) {ROS_INFO("Wait for 1 second before trying to unpause Gazebo again.");std::this_thread::sleep_for(std::chrono::seconds(1));unpaused = ros::service::call("/gazebo/unpause_physics", srv);++i;}if (!unpaused) {ROS_FATAL("Could not wake up Gazebo.");return -1;} else {ROS_INFO("Unpaused the Gazebo simulation.");}// Wait for 5 seconds to let the Gazebo GUI show up.ros::Duration(5.0).sleep();trajectory_msgs::MultiDOFJointTrajectory trajectory_msg;trajectory_msg.header.stamp = ros::Time::now();double begin_t = ros::Time::now().toSec();double t = 0;double angle = 0;ros::Rate rate(10);// Default desired position and yaw.Eigen::Vector3d desired_position(0.0, 0.0, 1.0);double desired_yaw = 0.0;// Overwrite defaults if set as node parameters.nh_private.param("x", desired_position.x(), desired_position.x());nh_private.param("y", desired_position.y(), desired_position.y());nh_private.param("z", desired_position.z(), desired_position.z());nh_private.param("yaw", desired_yaw, desired_yaw);while (ros::ok()){t = ros::Time::now().toSec() - begin_t;angle = fmod(2 * M_PI / 10 * t , 2 * M_PI); // 10s飞一圈desired_position.x() = cos(angle); // 圆的半径是1desired_position.y() = sin(angle);desired_position.z() = 1;desired_yaw = fmod(angle + M_PI/2 , 2 * M_PI);trajectory_msg.header.stamp = ros::Time::now();mav_msgs::msgMultiDofJointTrajectoryFromPositionYaw(desired_position, desired_yaw, &trajectory_msg);trajectory_pub.publish(trajectory_msg);rate.sleep();ros::spinOnce();}ros::shutdown();return 0;
}

修改~/UAV_rotors/src/rotors_simulator/rotors_gazebo/CMakeLists.txt，添加以下内容：

# 自己的文件
add_executable(run_circle_locus src/run_circle_locus.cpp)
target_link_libraries(run_circle_locus ${catkin_LIBRARIES})
add_dependencies(run_circle_locus ${catkin_EXPORTED_TARGETS})

重新编译一次程序：

cd ~/UAV_rotors
catkin build

(补充说明一下，修改launch文件不需要重新编译，但是修改源文件需要重新编译，每次编译时间会比较久，为了节约时间，我们可以指定编译规定功能包，如catkin build rotors_gazebo)

编译成功后，我们修改一下mav_hovering_example.launch文件，把里面启动的hovering_example节点改为我们写的节点程序：

<!-- <node name="hovering_example" pkg="rotors_gazebo" type="hovering_example" output="screen"/> -->
<node name="run_circle_locus" pkg="rotors_gazebo" type="run_circle_locus" output="screen"/>

ok，最后一步就是运行launch文件啦：

roslaunch rotors_gazebo mav_hovering_example.launch

结果图如下：

【rotors】多旋翼无人机仿真（二）——设置飞行轨迹相关推荐

【rotors】多旋翼无人机仿真（四）——参数补偿和PID控制
[rotors]多旋翼无人机仿真(一)--搭建rotors仿真环境 [rotors]多旋翼无人机仿真(二)--设置飞行轨迹 [rotors]多旋翼无人机仿真(三)--SE3控制 [rotors]多旋翼 ...
【rotors】多旋翼无人机仿真（三）——SE3控制
[rotors]多旋翼无人机仿真(一)--搭建rotors仿真环境 [rotors]多旋翼无人机仿真(二)--设置飞行轨迹 [rotors]多旋翼无人机仿真(三)--SE3控制 [rotors]多旋翼 ...
【rotors】多旋翼无人机仿真（一）——搭建rotors仿真环境
[rotors]多旋翼无人机仿真(一)--搭建rotors仿真环境 [rotors]多旋翼无人机仿真(二)--设置飞行轨迹 [rotors]多旋翼无人机仿真(三)--SE3控制 [rotors]多旋翼 ...
多旋翼无人机仿真 rotors_simulator：roll pitch yawrate thrust 控制器
多旋翼无人机仿真 rotors_simulator:roll pitch yawrate thrust 控制器前言 mav_with_keyboard.launch roll_pitch_yawra ...
【ROSGAZEBO】多旋翼无人机仿真（六）——SE(3)几何姿态控制器
[ROS&GAZEBO]多旋翼无人机仿真(一)--搭建仿真环境https://blog.csdn.net/qq_37680545/article/details/123185002 [ROS& ...
四旋翼无人机仿真之hector_quadrotor无人机（ROS + Gazebo）（三）传感器数据读取与复现（IMU、GPS）
系列文章目录文章1:四旋翼无人机仿真之hector_quadrotor无人机(ROS + Gazebo) 文章2:四旋翼无人机仿真之hector_quadrotor(二)键盘teleop_twist ...
多旋翼无人机仿真 rotors_simulator：用键盘控制无人机飞行
多旋翼无人机仿真 rotors_simulator:用键盘控制无人机飞行前言书接上文接口测试键盘指令发布指令转换与发布修改 rotors_simulator 的控制接口节点测试前言 R ...
四旋翼无人机仿真之hector_quadrotor无人机（ROS + Gazebo）（一）
这里写自定义目录标题应用环境 hector_quadrotor 功能包结构简介 hector_quadrotor 功能包安装 1. 安装所需依赖库 2. 下载.安装hector_quadrotor ...
多旋翼无人机仿真 rotors_simulator 是如何悬停的（一）
多旋翼无人机仿真 rotors_simulator 是如何悬停的(一) 前言 mav_hovering_example.launch mav_generic_odometry_sensor.gazeb ...

【rotors】多旋翼无人机仿真（二）——设置飞行轨迹

1、前言

2、源码解析

3、编写一个发布圆形轨迹的节点程序

【rotors】多旋翼无人机仿真（二）——设置飞行轨迹相关推荐

最新文章

热门文章