本教程建立在上一篇教程[ROS——无人机ROS仿真包 rotors_simulator 编译教程]的基础上,因此请先按照教程编译rotors_simulator 仿真包.本章教大家如何自己创建ROS包控制无人机运动. ~先上效果再讲解,兴趣来的更快些~ 下面进入实战
1.创建自己的ROS包
1.1 创建ROS包
先进入工程目录的src文件夹下,然后创建control包
cd UAV/src/
catkin_create_pkg control std_msgs rospy roscpp
1.2 添加rotors_simulator依赖
修改CMakeLists.txt文件: 修改前:
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
)
修改后:
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
sensor_msgs
geometry_msgs
mav_msgs
gazebo_msgs
)
其中 \bulletsensor_msgs,添加对ROS官方sensor_msgs消息的依赖,在包的程序中可以使用sensor_msgs/xxx类型的消息; \bulletgeometry_msgs,添加对ROS官方geometry_msgs消息的依赖,在包的程序中可以使用geometry_msgs/xxx类型的消息; \bulletmav_msgs,添加对mav_msgs消息的依赖,在包的程序中可以使用mav_msgs/xxx类型的消息,该消息的定义在UAV\src\mav_comm包中定义; \bulletgazebo_msgs添加对ROS官方gazebo_msgs消息的依赖,在包的程序中可以使用gazebo_msgs/xxx类型的消息,主要用于从Gazebo中获取消息. 修改package.xml文件,在其中添加:
<depend>gazebo_msgs</depend>
<depend>gazebo_plugins</depend>
<depend>geometry_msgs</depend>
<depend>joy</depend>
<depend>mav_msgs</depend>
<depend>rotors_gazebo_plugins</depend>
<depend>sensor_msgs</depend>
<depend>xacro</depend>
添加后如下图所示:
1.3 创建.cpp文件
在UAV/src文件夹中创建control.cpp文件,用于编写控制代码,添加后的工程架构如下图所示
在CMakeLists.txt文件最下方添加.cpp文件的编译依赖信息:
add_executable(control src/control.cpp)
target_link_libraries(control ${catkin_LIBRARIES})
2. 控制代码
1）初始化ROS节点； 2）创建position_sub订阅获取无人机位置信息的话题 \bullet话题名称为/odometry_sensor1/position，可以通过rqt查询到其他话题名称及对应的消息数据类型； \bullet回调函数为updateUavPosition，位置信息类型为geometry_msgs::PointStamped； \bullet注意一个细节，当GPS初始化成功，接收到位置信息时才开始执行任务。 3）创建发布控制指令的publisher，命名为trajectory_pub \bullettrajectory_pub = nh.advertise(mav_msgs::default_topics::COMMAND_TRAJECTORY, 10); \bullet在此控制无人机的移动借助于rotors_simulator包中的控制节点，因此publisher中的话题为rotors_simulator包中定义的话题； \bullet在此使用rotors_simulator包中的控制节点只能对无人机位置进行控制，也就是发送指定位置点到该话题即可。但是需要注意，如果目标位置和当前位置距离较远时，无人机会侧翻； \bullet为避免目标点较远时无人机侧翻，这里定义了linearSmoothingNavigationTask()函数来平滑轨迹，该函数中将目标点到当前位置的距离分段，并计算出下一步要到达的临时目标点，此时函数返回0。多次调用该函数后才可到达目标点，此时函数返回1。 我已经尽可能详细的将代码进行了注释,还有疑问的请留言.
#include <ros/ros.h>
#include <iostream>
#include <chrono>
#include <thread>
#include <geometry_msgs/PointStamped.h>
#include <std_srvs/Empty.h>
#include <Eigen/Core>
#include <mav_msgs/conversions.h>
#include <mav_msgs/default_topics.h>
#include <trajectory_msgs/MultiDOFJointTrajectory.h>
ros::Publisher trajectory_pub;
geometry_msgs::PointStamped current_position;
float linear_smoothing_navigation_step = 2;
bool flag_gps_initialized_OK = false;
bool flag_take_off_OK = false;
int flag_tasks_OK = 0;
Eigen::Vector3d home;
/*
Description:
updateUavPosition(const geometry_msgs::PointStamped& msg)
gps数据更新的回调函数.
Parameters:
msg 位置信息
Return:
无
*/
void updateUavPosition(const geometry_msgs::PointStamped& msg)
{
if (!flag_gps_initialized_OK)
{
flag_gps_initialized_OK= true;
home[0] = msg.point.x;
home[1] = msg.point.y;
home[2] = msg.point.z;
}
current_position = msg;
// std::cout<<"UAV current position is: "<<msg.point.x<< msg.point.y<< msg.point.z<<std::endl;
}
/*
Description:
getDistanceToTarget(const Eigen::Vector3d& target)
获取当前位置到指定位置位置的距离.
Parameters:
target 需要飞达的位置点
Return:
double 当前位置到达目标点的位置
*/
double getDistanceToTarget(const Eigen::Vector3d& target)
{
double temp = 0;
temp += pow((target[0] - current_position.point.x), 2);
temp += pow((target[1] - current_position.point.y), 2);
temp += pow((target[2] - current_position.point.z), 2);
temp = sqrt(temp);
return temp;
}
/*
Description:
reachTargetPosition(const Eigen::Vector3d& target, float max_error)
判定是否到达指定的目标点.
Parameters:
target 需要飞达的位置点
max_error 允许的位置误差阈值,当前位置和目标位置小于该阈值时,判定无人机到达目标点
Return:
bool 到达目标点时返回 true
未到达目标点时返回 false
*/
bool reachTargetPosition(const Eigen::Vector3d& target, float max_error)
{
double temp = getDistanceToTarget(target);
if (temp < max_error)
return true;
return false;
}
/*
Description:
linearSmoothingNavigationTask(const Eigen::Vector3d& target)
控制无人机从当前位置飞向指定位置.
Parameters:
target 需要飞达的位置点
Return:
bool 起飞结束后返回 true
起飞过程中返回 false
*/
bool linearSmoothingNavigationTask(const Eigen::Vector3d& target)
{
trajectory_msgs::MultiDOFJointTrajectory trajectory_msg;
trajectory_msg.header.stamp = ros::Time::now();
if (reachTargetPosition(target,0.2))
return true;
double dist = getDistanceToTarget(target);
Eigen::Vector3d next_step;
if(dist<linear_smoothing_navigation_step)
{
next_step = target;
}
else
{
next_step[0] = current_position.point.x+(target[0]-current_position.point.x)/dist*linear_smoothing_navigation_step;
next_step[1] = current_position.point.y+(target[1]-current_position.point.y)/dist*linear_smoothing_navigation_step;
next_step[2] = current_position.point.z+(target[2]-current_position.point.z)/dist*linear_smoothing_navigation_step;
}
double desired_yaw = 0.0;
mav_msgs::msgMultiDofJointTrajectoryFromPositionYaw(next_step, desired_yaw, &trajectory_msg);
trajectory_pub.publish(trajectory_msg);
return false;
}
/*
Description:
takeOffTask(float height)
起飞函数,调用后无人机从起始位置起飞指定高度.
Parameters:
height 指定的起飞高度
Return:
bool 起飞结束后返回 true
起飞过程中返回 false
*/
bool takeOffTask(float height)
{
trajectory_msgs::MultiDOFJointTrajectory trajectory_msg;
trajectory_msg.header.stamp = ros::Time::now();
static Eigen::Vector3d desired_position(current_position.point.x, current_position.point.y, height);
double desired_yaw = 0.0;
if (reachTargetPosition(desired_position,0.2))
return true;
mav_msgs::msgMultiDofJointTrajectoryFromPositionYaw(desired_position, desired_yaw, &trajectory_msg);
trajectory_pub.publish(trajectory_msg);
return false;
}
/*
Description:
gohome()
反航函数,调用后无人机先沿着当前高度飞到反航点正上方,然后降落.
Parameters:
无
Return:
无
*/
void gohome()
{
static Eigen::Vector3d temp(home[0], home[1], current_position.point.z);
static bool flag_temp = false;
if (!flag_temp)
{
flag_temp = linearSmoothingNavigationTask(temp);
}
else
{
linearSmoothingNavigationTask(home);
}
}
int main(int argc, char** argv) {
ros::init(argc, argv, "UAV_Controler");
ros::NodeHandle nh;
// Create a private node handle for accessing node parameters.
ros::NodeHandle nh_private("~");
std::string uav_name = "";
ros::param::get("~mav_name",uav_name);
// 订阅话题
// /odometry_sensor1/position 无人机位置信息(包含噪声)
ros::Subscriber position_sub = nh.subscribe(std::string("/"+uav_name+"/odometry_sensor1/position").c_str(), 10, &updateUavPosition);
trajectory_pub = nh.advertise<trajectory_msgs::MultiDOFJointTrajectory>(mav_msgs::default_topics::COMMAND_TRAJECTORY, 10);
// 等待5s,让Gazebo可以成功启动.
ros::Duration(5.0).sleep();
// 创建控制Gazebo自动运行的服务,这里自动运行是指让Gazebo自动Play
std_srvs::Empty srv;
bool unpaused = ros::service::call("/gazebo/unpause_physics", srv);
// 尝试让Gazebo自动运行
int i=0;
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;
}
// 判断Gazebo有没有自动运行,没有成功Play则退出
if (!unpaused) {
ROS_FATAL("Could not wake up Gazebo.");
return -1;
} else {
ROS_INFO("Unpaused the Gazebo simulation.");
}
std::vector<Eigen::Vector3d> path;
path.push_back(Eigen::Vector3d(5.f,5.f,5.f));
path.push_back(Eigen::Vector3d(-5.f,5.f,5.f));
path.push_back(Eigen::Vector3d(-5.f,-5.f,5.f));
path.push_back(Eigen::Vector3d(5.f,-5.f,5.f));
path.push_back(Eigen::Vector3d(5.f,5.f,5.f));
std::cout << path.size() << std::endl;
ros::Rate loop_rate(10);
while (ros::ok())
{
if(flag_gps_initialized_OK && !flag_take_off_OK)
{
// ROS_INFO("UAV take off task is running...");
flag_take_off_OK = takeOffTask(3);
}
else if(flag_take_off_OK && flag_tasks_OK<path.size())
{
if(flag_tasks_OK<path.size())
{
bool temp = linearSmoothingNavigationTask(path[flag_tasks_OK]);
if (temp)
flag_tasks_OK ++;
}
}
else if(flag_tasks_OK >= path.size())
{
gohome();
}
ros::spinOnce();
loop_rate.sleep();
}
}
3. launch配置
在工程目录下创建launch文件夹，并在其中添加uav.launch文件，在其中添加下属内容。
<launch>
<arg name="mav_name" default="firefly"/>
<arg name="world_name" default="basic"/>
<arg name="enable_logging" default="false" />
<arg name="enable_ground_truth" default="true" />
<arg name="log_file" default="$(arg mav_name)" />
<arg name="debug" default="false"/>
<arg name="gui" default="true"/>
<arg name="paused" default="false"/>
<!-- The following line causes gzmsg and gzerr messages to be printed to the console
(even when Gazebo is started through roslaunch) -->
<arg name="verbose" default="false"/>
<!--Run Gazebo-->
<env name="GAZEBO_MODEL_PATH" value="${GAZEBO_MODEL_PATH}:$(find rotors_gazebo)/models"/>
<env name="GAZEBO_RESOURCE_PATH" value="${GAZEBO_RESOURCE_PATH}:$(find rotors_gazebo)/models"/>
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(find rotors_gazebo)/worlds/$(arg world_name).world" />
<arg name="debug" value="$(arg debug)" />
<arg name="paused" value="$(arg paused)" />
<arg name="gui" value="$(arg gui)" />
<arg name="verbose" value="$(arg verbose)"/>
</include>
<!--Run UAV model and control node-->
<group ns="$(arg mav_name)">
<include file="$(find rotors_gazebo)/launch/spawn_mav.launch">
<arg name="mav_name" value="$(arg mav_name)" />
<arg name="model" value="$(find rotors_description)/urdf/mav_generic_odometry_sensor.gazebo" />
<arg name="enable_logging" value="$(arg enable_logging)" />
<arg name="enable_ground_truth" value="$(arg enable_ground_truth)" />
<arg name="log_file" value="$(arg log_file)"/>
</include>
<node name="lee_position_controller_node" pkg="rotors_control" type="lee_position_controller_node" output="screen">
<rosparam command="load" file="$(find rotors_gazebo)/resource/lee_controller_$(arg mav_name).yaml" />
<rosparam command="load" file="$(find rotors_gazebo)/resource/$(arg mav_name).yaml" />
<remap from="odometry" to="odometry_sensor1/odometry" />
</node>
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<node
name="control" pkg="control" type="control" output="screen">
<param name="mav_name" type="string" value="$(arg mav_name)"/>
</node>
</group>
</launch>
4. 编译&运行
catkin_make
source devel/setup.bash
roslaunch uav.launch
然后你就可以看到程序自动启动Gazebo，并且无人机可以开始运动咯~
4. git源码
懒小象的github: https://github.com/MeMiracle/UAV
开心洋葱 , 版权所有丨如未注明 , 均为原创丨未经授权请勿修改 , 转载请注明ROS——无人机ROS仿真包 rotors_simulator 使用教程！
