ROS-Kinetic中基于hector_quadrotor的多无人机仿真

于 2020-07-09 12:41:09 发布
阅读量606 收藏 5
点赞数
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.youkuaiyun.com/nm46nm/article/details/79065186
版权

安装ROS-Kinetic

1.设置更新

2.添加软件源

sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'

3.安装密钥

sudo apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net:80 --recv-key 421C365BD9FF1F717815A3895523BAEEB01FA116

4.更新软件源

sudo apt-get update

5.安装ROS-Kinetic

sudo apt-get install ros-kinetic-desktop-full

    注意：如果本机已装有最新版gazebo，请卸载后再执行这一步，因为ros的desktop-full版本中已经有了gazebo，且其版本较旧，因而二者会发生冲突。当然你也可以选择安装其他版本的ros，详见http://wiki.ros.org/kinetic/Installation/Ubuntu

6.初始化与环境设置

sudo rosdep init
rosdep update

echo "source /opt/ros/kinetic/setup.bash" >> ~/.bashrc
source ~/.bashrc

想要继续研究ROS可参考https://www.shiyanlou.com/courses/854

安装hector_quadrotor

1.安装

mkdir -p hector_quadrotor_tutorial/src
cd hector_quadrotor_tutorial
wstool init src https://raw.github.com/tu-darmstadt-ros-pkg/hector_quadrotor/kinetic-devel/tutorials.rosinstall

2.安装依赖项

rosdep install --from-path src --ignore-src


这一命令会安装所有的依赖项。当然，如果编译时发现还是出现依赖问题如下图

可以从源：https://github.com/ros-geographic-info/geographic_info.git下载，解压到hector_quadrotor_tutorial/src目录下

其他依赖问题可以取github上寻找相应的包，但是一般不会出现，如果执行安装所有依赖的命令的话。

3.编译

cd hector_quadrotor_tutorial
catkin_make

之后记得在hector_quadrotor_tutorial目录下设置环境

source devel/setup.bash

4.运行

可以先运行自带的demo试试：

roslaunch hector_quadrotor_demo indoor_slam_gazebo.launch

5.多机launch文件

在/home/usr_name/hector_quadrotor_tutorial/src/hector_quadrotor/hector_quadrotor_demo目录下，打开launch文件夹，会看到刚刚运行的indoor_slam的launch文件，打开可以查看其源代码。多机代码可如下：

<?xml version="1.0"?>

<launch>
   <include file="$(find gazebo_ros)/launch/empty_world.launch"/>
   <arg name="model" default="$(find hector_quadrotor_description)/urdf/quadrotor_hokuyo_utm30lx.gazebo.xacro"/>

   <group ns="quad1">
     <include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
       <arg name="name" value="quad1" />
       <arg name="tf_prefix" value="quad1" />
       <arg name="model" value="$(arg model)" />
       <arg name="x" value="1.5" />
       <arg name="y" value="5" />
       <arg name="z" value="0.3" />
     </include>
   </group>

   <group ns="quad2">
     <include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
       <arg name="name" value="quad2" />
       <arg name="tf_prefix" value="quad2" />
       <arg name="model" value="$(arg model)" />
       <arg name="x" value="14.5" />
       <arg name="y" value="9" />
       <arg name="z" value="0.3" />
     </include>
   </group>

   <group ns="quad3">
     <include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
       <arg name="name" value="quad3" />
       <arg name="tf_prefix" value="quad3" />
       <arg name="model" value="$(arg model)" />
       <arg name="x" value="5" />
       <arg name="y" value="3.5" />
       <arg name="z" value="0.3" />
     </include>
   </group>

<group ns="quad4">
     <include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
       <arg name="name" value="quad4" />
       <arg name="tf_prefix" value="quad4" />
       <arg name="model" value="$(arg model)" />
       <arg name="x" value="4" />
       <arg name="y" value="7.5" />
       <arg name="z" value="0.3" />

     </include>
   </group>

<group ns="quad5">
     <include file="$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch">
       <arg name="name" value="quad5" />
       <arg name="tf_prefix" value="quad5" />
       <arg name="model" value="$(arg model)" />
       <arg name="x" value="7.77" />
       <arg name="y" value="2.23" />
       <arg name="z" value="0.3" />
     </include>
   </group>



</launch>


比较简单，不解释。这是5架无人机的代码，可以根据需要增减。在launch文件夹中新建一个yourname.launch文件，复制上面代码进去。可以尝试运行一下试试，

roslaunch hector_quadrotor_demo yourname.launch

按ctrl+c可结束运行。

6.控制代码

在hector_quadrotor_demo文件夹中，打开src文件夹，新建yourcpp.cpp文件，复制粘贴一下内容：

   #include "ros/ros.h"
   #include "std_msgs/String.h"
   #include "geometry_msgs/Twist.h"
   #include "nav_msgs/Odometry.h"
   #include <string>
   #include "/usr/include/c++/5/bits/stringfwd.h"
   #include "stdlib.h"
   #include <sstream>
   #include "math.h"
   #include <message_filters/subscriber.h>
   #include <message_filters/time_synchronizer.h>
   #define PI 3.1415926
   using namespace message_filters;
   const int uav_num = 5;
   const int d_fre = 50;
   int count = 0;
   float t = 0, t0 = 0, pt = 0, dt = 0;
   float pose[uav_num][3], vel[uav_num][3];

   void sort(int* index, float p_bar[][2])
   {
       float a[uav_num], tem = 2*PI;
       int b[uav_num] = {0}, t = 0;
       for(int k = 0; k < uav_num; k++)
       {
           a[k] = atan2(p_bar[k][1], p_bar[k][0]);
       }
       for(int i = 0; i < uav_num; i++)
       {
           for(int j = 0; j < uav_num; j++)
           {
               if(a[j]<tem && b[j]==0)
               {
                   tem = a[j];
                   t = j;
               }
           }
           index[i] = t;
           b[t] = 1;
           tem = 2*PI;
       }
   }

   void chatterCallback(const nav_msgs::Odometry::ConstPtr& msg)
   {
       int id = 0;
       char s[20] = "hello";
       strcpy(s, msg->header.frame_id.c_str());
       sscanf(&s[4], "%d", &id);
       ROS_INFO("%s ***%d",s, id);
       pose[id-1][0] = msg->pose.pose.position.x;
       pose[id-1][1] = msg->pose.pose.position.y;
       pose[id-1][2] = msg->pose.pose.position.z;
       vel[id-1][0] = msg->twist.twist.linear.x;
       vel[id-1][1] = msg->twist.twist.linear.y;
       vel[id-1][2] = msg->twist.twist.linear.z;
       t = msg->header.stamp.sec + (1e-9) * msg->header.stamp.nsec;
       if(count == 0) t0 = t;
       count++;

   }


   int main(int argc, char **argv)
   {

     ros::init(argc, argv, "cmd");
     ros::NodeHandle n; //g[4];
     ros::Rate loop_rate(uav_num*d_fre);
     ros::Publisher cmd_pub[uav_num];
     ros::Subscriber cmd_sub[uav_num];
     ros::ServiceClient enable_motors_client[uav_num];
     char  **cmd_tpc, **stat_tpc, **motor, tem1[30]="",tem2[30]="";
     cmd_tpc = new char* [uav_num];
     stat_tpc = new char* [uav_num];
     motor = new char* [uav_num];

     hector_uav_msgs::EnableMotors motor_cmd;
     motor_cmd.request.enable = true;

     for( int ni = 0; ni < uav_num; ni++)
     {
         cmd_tpc[ni] = (char*)malloc(30*sizeof(char));
         stat_tpc[ni] = (char*)malloc(30*sizeof(char));
         motor[ni] = (char*)malloc(30*sizeof(char));
         //itoa(i+1,cmd_tpc,20);
         sprintf(tem1, "%d",ni+1);
         strcpy(tem2 ,"quad");
         strcpy(tem1 , strcat(tem2,tem1));
         strcpy(tem2 , "/cmd_vel");
         strcpy(cmd_tpc[ni],strcat(tem1,tem2));
         //cmd_tpc = "quad"+intToStr(i+1)+"/cmd_vel";
         cmd_pub[ni] = n.advertise<geometry_msgs::Twist>(cmd_tpc[ni], 1000);

         sprintf(tem1, "%d",ni+1);
         strcpy(tem2, "quad");
         strcpy(tem1, strcat(tem2,tem1));
         strcpy(tem2, "/ground_truth/state");
         strcpy(stat_tpc[ni], strcat(tem1,tem2));
         cmd_sub[ni] = n.subscribe(stat_tpc[ni], 1000, chatterCallback);

         sprintf(tem1,"%d",ni+1);
         strcpy(tem2,"quad");
         strcpy(tem1, strcat(tem2,tem1));
         strcpy(tem2, "/enable_motors");
         strcpy(motor[ni], strcat(tem1,tem2));
         enable_motors_client[ni] = n.serviceClient<hector_uav_msgs::EnableMotors>
                 (motor[ni]);

     }

     ros::spinOnce();
     geometry_msgs::Twist msg;

     while(t-t0<3)
     {
       if(!ros::ok()) break;
       int ni = 0;
       for(ni = 0; ni < uav_num; ni++)
       {
           enable_motors_client[ni].call(motor_cmd);
           ros::spinOnce();
           loop_rate.sleep();

           ROS_INFO("###@[%d]",ni);
           ROS_INFO("^^^^^{%lf}****[%lf]", t-t0, t);
           ROS_INFO("pose:n = [%d],  [%lf,%lf,%lf]\n",ni+1,pose[ni][0],pose[ni][1],pose[ni][2]);
           ROS_INFO("velosity:n = [%d],  [%lf,%lf,%lf]\n",ni+1,vel[ni][0],vel[ni][1],vel[ni][2]);
       }

     }

     float c1 = 0.4, c2 = 1, lambda = 1, gamma = 1;
     float r = 2.5, cen[2] = {5,5}, v0[2] = {0,0};
     //float d[5] = {1.1469, 1.0034, 2.1867, 0.5395, 1.4068};
     float d[uav_num] = {0};
     int index[uav_num];
 
  while(t-t0<5)
     {
       if(!ros::ok()) break;
       int ni = 0;
       for(ni = 0; ni < uav_num; ni++)
       {


           msg.linear.x = 0;
           msg.linear.y = 0;
           msg.linear.z = 2.0;
           msg.angular.x = 0;
           msg.angular.y = 0;
           msg.angular.z = 0;
           cmd_pub[ni].publish(msg);


           ros::spinOnce();
           loop_rate.sleep();

           ROS_INFO("###@[%d]",ni);
           ROS_INFO("^^^^^{%f}****[%f]", t-t0, t);
           ROS_INFO("pose:n = [%d],  [%f,%f,%f]\n",ni+1,pose[ni][0],pose[ni][1],pose[ni][2]);
           ROS_INFO("velosity:n = [%d],  [%f,%f,%f]\n",ni+1,vel[ni][0],vel[ni][1],vel[ni][2]);
       }

     }
     ROS_INFO(">>>>>>>>>>>>1");
     while(t-t0<8)
     {
       if(!ros::ok()) break;
       int ni = 0;
       for(ni = 0; ni < uav_num; ni++)
       {


           msg.linear.x = 0;
           msg.linear.y = 0;
           msg.linear.z = 0;
           msg.angular.x = 0;
           msg.angular.y = 0;
           msg.angular.z = 0;
           cmd_pub[ni].publish(msg);


           ros::spinOnce();
           loop_rate.sleep();

           ROS_INFO("###@[%d]&&",ni);
           ROS_INFO("^^^^^{%f}****[%f]", t-t0, t);
           ROS_INFO("pose:n = [%d],  [%f,%f,%f]\n",ni+1,pose[ni][0],pose[ni][1],pose[ni][2]);
           ROS_INFO("velosity:n = [%d],  [%f,%f,%f]\n",ni+1,vel[ni][0],vel[ni][1],vel[ni][2]);
       }

     }
     ROS_INFO(">>>>>>>>>>>>1.5");
     //************************************************************************
     float p_bar[uav_num][2], l = 0, f = 0;
     int i_plus = 0, i_sub = 0;
     float theta = 0, theta_plus = 0, theta_sub = 0, alpha = 0, alpha_sub = 0;

     for(int i = 0; i < uav_num; i++)
     {
         p_bar[i][0] = pose[i][0] - cen[0];
         p_bar[i][1] = pose[i][1] - cen[1];
         d[i] = 2*PI/uav_num;
     }

     sort(index,p_bar);
     float nl,omig = 0.1;
     while(ros::ok())
     {
         int ni = 0;
         dt = t - pt;
         pt = t;
         for(int i = 0; i < uav_num; i++)
         {
             p_bar[i][0] = pose[i][0] - cen[0];
             p_bar[i][1] = pose[i][1] - cen[1];
         }
         for(ni = 0; ni < uav_num; ni++)
         {

             l = r*r - (p_bar[index[ni]][0]*p_bar[index[ni]][0]+p_bar[index[ni]][1]*p_bar[index[ni]][1]);
             nl = r - sqrt(p_bar[index[ni]][0]*p_bar[index[ni]][0]+p_bar[index[ni]][1]*p_bar[index[ni]][1]);
             if(fabs(nl)<1) omig = 1;
             else omig = 0.1;
             //ROS_INFO("pbarx=%f,  pbary=%f,  r=%f,   l=%f",p_bar[index[ni]][0],p_bar[index[ni]][1])
             i_plus = (ni+1)%uav_num;
             i_sub = (ni-1+uav_num)%uav_num;
//             if(ni==1) i_sub = uav_num;
//             if(ni==uav_num) i_plus = 1;
             theta_plus = atan2(p_bar[index[i_plus]][1],p_bar[index[i_plus]][0]);
             //if(theta_plus<0) theta_plus+=2*PI;
             theta = atan2(p_bar[index[ni]][1],p_bar[index[ni]][0]);
             //if(theta<0) theta+=2*PI;
             theta_sub = atan2(p_bar[index[i_sub]][1],p_bar[index[i_sub]][0]);
             //if(theta_sub<0) theta_sub+=2*PI;
             ROS_INFO("theta_plus=%f,  theta=%f,  theta_sub=%f",theta_plus,theta,theta_sub);
             alpha = theta_plus - theta;
             if(alpha<0) alpha += 2*PI;
             alpha_sub = theta - theta_sub;
             if(alpha_sub<0) alpha_sub += 2*PI;
             f = c1 + c2/(2*PI) * (d[index[i_sub]]*alpha-d[index[ni]]*alpha_sub)/(d[index[ni]]+d[index[i_sub]]);

             msg.linear.x = lambda*f*(gamma*l*p_bar[index[ni]][0]-p_bar[index[ni]][1])*omig;
             msg.linear.y = lambda*f*(p_bar[index[ni]][0]+gamma*l*p_bar[index[ni]][1])*omig;
             msg.linear.z = 0;
             msg.angular.x = 0;
             msg.angular.y = 0;
             msg.angular.z = 0;

             ROS_INFO("###@ni=[%d]-------------------index=%d-------------------",ni,index[ni]);
             ROS_INFO("l=%f,  i_plus=%d, i_sub=%d,  alpha=%f, alpha_sub=%f,",l,i_plus,i_sub,alpha,alpha_sub);
             ROS_INFO(" f=%f,  vx=%f,  vy=%f",f,msg.linear.x,msg.linear.y);

             cmd_pub[index[ni]].publish(msg);


             ros::spinOnce();
             loop_rate.sleep();

             ROS_INFO("^^^^^{%f}****[%f]&&&&&&{%f}", t-t0, t, dt);
             ROS_INFO("pose:n = [%d],  [%f,%f,%f]\n",ni+1,pose[ni][0],pose[ni][1],pose[ni][2]);
             ROS_INFO("velosity:n = [%d],  [%f,%f,%f]\n",ni+1,vel[ni][0],vel[ni][1],vel[ni][2]);
         }


         ROS_INFO(">>>>>>>>>>>>10");
     }

    return 0;
  }

这是环形编队的代码，注意其中的uav_num变量是无人机数量，需要与launch文件中的数量一致。关于ROS话题与消息机制的相关知识，可以去https://www.shiyanlou.com/courses/854进行学习。

7.修改CmakeList文件

在hector_quadrotor_demo文件夹中找到CmakeList.txt文件，用gedit打开，在末尾添加如下代码：

include_directories(include ${catkin_INCLUDE_DIRS})

add_executable(yourcpp src/yourcpp.cpp) 
target_link_libraries(yourcpp ${catkin_LIBRARIES}) 
add_dependencies(yourcpp hector_quadrotor_demo_generate_messages_cpp)

注意，这里的‘yourcpp’和之前所有的‘yourname’均指的是你新建文件的名字，‘yourcpp’即你新建cpp文件的名字，yourname是你新建launch文件的名字。

8.修改package.xml文件

同样在hector_quadrotor_demo文件夹中找到package.xml文件，打开，修改为如下样子：

<package>
  <name>hector_quadrotor_demo</name>
  <version>0.3.5</version>
  <description>hector_quadrotor_demo contains various launch files and needed dependencies for demonstration of the hector_quadrotor stack (indoor/outdoor flight in gazebo etc.)</description>
  <maintainer email="meyer@fsr.tu-darmstadt.de">Johannes Meyer</maintainer>

  <license>BSD</license>

  <url type="website">http://ros.org/wiki/hector_quadrotor_demo</url>
  <url type="bugtracker">https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor/issues</url>

  <author email="kohlbrecher@sim.tu-darmstadt.de">Stefan Kohlbrecher</author>

  <!-- Dependencies which this package needs to build itself. -->
  <buildtool_depend>catkin</buildtool_depend>

  <!-- Dependencies needed to compile this package. -->
  <!--添加项 -->
  <build_depend>roscpp</build_depend>
  <build_depend>rospy</build_depend>
  <build_depend>geometry_msgs</build_depend>
  <build_depend>nav_msgs</build_depend>

  <!--添加项 -->
  <!-- Dependencies needed after this package is compiled. -->
  <run_depend>hector_quadrotor_gazebo</run_depend>
  <run_depend>hector_gazebo_worlds</run_depend>
  <run_depend>hector_mapping</run_depend>
  <run_depend>hector_geotiff</run_depend>
  <run_depend>hector_trajectory_server</run_depend>
  <!--添加项 -->
  <run_depend>roscpp</run_depend>
  <run_depend>rospy</run_depend>
  <run_depend>geometry_msgs</run_depend>
  <build_depend>nav_msgs</build_depend>
  <run_depend>message_runtime</run_depend>
  <!--添加项 -->
  <!-- Dependencies needed only for running tests. -->

</package>


其中注有‘添加项’的应该是要修改的部分。

9.编译（catkin_make)和source

catkin_make
source devel/setup.bash

编译过程可能会出错，根据错误提示，可视情况做如下修改：

在上述.cpp文件加上头文件 #include "hector_uav_msgs/EnableMotors.h"

在Cmakelist.txt 文件里find packages项改成:

find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs geometry_msgs nav_msgs hector_uav_msgs genmsg actionlib sensor_msgs tf)

在include_diretories项里加上 $ {PCL_INCLUDE_DIRS}

在package.xml文件里加上:

<run_depend>项加 tf、nav_msgs、actionlib、actionlib_msgs

<build_depend>项加 actionlib、acrionlib_msgs

注意修改后需要重新编译和source

10.仿真

打开终端，运行你新建的launch文件：

roslaunch hector_quadrotor_demo yourname.launch

再开一个终端，运行代码：

rosrun hector_quadrotor_demo yourname

即可在gazebo中看到多无人机环形编队结果。

参考：1. http://blog.youkuaiyun.com/yjy728/article/details/75201110/#42-%E5%BC%80%E5%A7%8B%E4%BB%BF%E7%9C%9F

           2. http://blog.youkuaiyun.com/phenixlou/article/details/78518207

          以及文中列出的网址。  

nm46nm
关注
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值