赞
踩
首先需要建立ROS软件包,并下载对应的Rotors的软件功能包:
$ mkdir -p /ROS/rotors_ws/src
$ cd ROS/rotors_ws/src
$ catkin_init_workspace
$ git clone https://github.com/xmy0916/rotors
之后编译git下来的代码即可:
$ cd ..
$ catkin_make
$ source devel/setup.sh
Firefly机器人是一个六轴的机器人,带有三个摄像头,从rviz的topic中可以看到有深度、RGB双目摄像头。出于个人喜好,可以对launch文件中的world进行修改,选择不同的运行物理环境,我喜欢沙都没有。所以,用了basic.world来对无人机进行仿真。订阅后的摄像头图像会发现看到了上面螺旋桨的旋转,所以我在mav_with_vi_sensor.gazebo
文件中对摄像头的位置进行了微调,为以后的建图以及图像识别的容错率提升打好基础。
在调整好摄像头的角度后,遇到了其他插件无法转换至bask_link
上的问题,需要将坐标系换成world
坐标系,来完成转换。
首先在控制交互上我选用的是通过扫描键盘按下的键值,然后进行对应的控制。所以识别被按下键盘值成为首要解决。
#include <fstream> #include <iostream> #include <Eigen/Core> #include <mav_msgs/conversions.h> #include <mav_msgs/default_topics.h> #include <ros/ros.h> #include <trajectory_msgs/MultiDOFJointTrajectory.h> #include <stdio.h> #include <termios.h> static struct termios initial_settings, new_settings; static int peek_character = -1; void init_keyboard(void); void close_keyboard(void); int kbhit(void); int readch(void); void init_keyboard() { tcgetattr(0,&initial_settings); new_settings = initial_settings; new_settings.c_lflag |= ICANON; new_settings.c_lflag |= ECHO; new_settings.c_lflag |= ISIG; new_settings.c_cc[VMIN] = 1; new_settings.c_cc[VTIME] = 0; tcsetattr(0, TCSANOW, &new_settings); } void close_keyboard() { tcsetattr(0, TCSANOW, &initial_settings); } int kbhit() { unsigned char ch; int nread; if (peek_character != -1) return 1; new_settings.c_cc[VMIN]=0; tcsetattr(0, TCSANOW, &new_settings); nread = read(0,&ch,1); new_settings.c_cc[VMIN]=1; tcsetattr(0, TCSANOW, &new_settings); if(nread == 1) { peek_character = ch; return 1; } return 0; } int readch() { char ch; if(peek_character != -1) { ch = peek_character; peek_character = -1; return ch; } read(0,&ch,1); return ch; } int main(int argc, char** argv) { init_keyboard(); ros::init(argc, argv, "waypoint_publisher"); ros::NodeHandle nh("//firefly"); ros::Publisher trajectory_pub = nh.advertise<trajectory_msgs::MultiDOFJointTrajectory>(mav_msgs::default_topics::COMMAND_TRAJECTORY, 10); ROS_INFO("Started waypoint_publisher."); const float DEG_2_RAD = M_PI / 180.0; float pos_x = 0,pos_y = 0,pos_z = 1; float yaw_deg = 0; int isChange = 1; while(ros::ok()) { if(kbhit()) { int key = readch(); switch(key) { case 56: pos_z += 0.5; isChange = 1; printf("%d\n",key); break; case 50://2 pos_z -= 0.5; isChange = 1; printf("%d\n",key); break; case 97://a pos_y += 0.5; isChange = 1; printf("%d\n",key); break; case 100://d pos_y -= 0.5; isChange = 1; printf("%d\n",key); break; case 119://w pos_x += 0.5; isChange = 1; printf("%d\n",key); break; case 120://x pos_x -= 0.5; isChange = 1; printf("%d\n",key); break; default: printf("%d\n",key); break; } } if(isChange == 1) { trajectory_msgs::MultiDOFJointTrajectory trajectory_msg; trajectory_msg.header.stamp = ros::Time::now(); Eigen::Vector3d desired_position(pos_x, pos_y,pos_z); double desired_yaw = yaw_deg * DEG_2_RAD; mav_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); isChange = 0; } ros::spinOnce(); } ros::shutdown(); return 0; }
程序中首先定义了三个函数:void init_keyboard(void)
、void close_keyboard(void)
和int kbhit(void)
,第一个是用来对键盘的初始化、第二个函数是tcsetattr
是用于设置终端参数的函数,而第三个函数的存在原因是,Windows系统下可以使用_kbhit()函数来获取键盘事件,来判断是否有按键被按下,而Unix/Linux中,没有提供kbhit()函数,所以我们需要自己来实现kbhit()程序。
在键盘控制中我们设定了6个键,分别对应前进、后退、向左、向右、上升和下降,没有加入旋转选项。在分析发布数据格式时候进行讨论。对控制话题发布的格式是ros::Publisher trajectory_pub = nh.advertise <trajectory_msgs::MultiDOFJointTrajectory>(mav_msgs::default_topics::COMMAND_TRAJECTORY, 10);
,所以根据该代码可以发现话题是:COMMAND_TRAJECTORY
且数据格式是trajectory_msgs::MultiDOFJointRrajectory
通过rosmsg show
来查询该数据格式的具体内容
$ rosmsg show trajectory_msgs/MultiDOFJointTrajectory std_msgs/Header header uint32 seq time stamp string frame_id string[] joint_names trajectory_msgs/MultiDOFJointTrajectoryPoint[] points geometry_msgs/Transform[] transforms geometry_msgs/Vector3 translation float64 x float64 y float64 z geometry_msgs/Quaternion rotation float64 x float64 y float64 z float64 w geometry_msgs/Twist[] velocities geometry_msgs/Vector3 linear float64 x float64 y float64 z geometry_msgs/Vector3 angular float64 x float64 y float64 z geometry_msgs/Twist[] accelerations geometry_msgs/Vector3 linear float64 x float64 y float64 z geometry_msgs/Vector3 angular float64 x float64 y float64 z duration time_from_start
根据上面的数据格式的结果显示进行分析,其消息格式下主要涵盖了两个类型的消息:string[] joint_names
和trajectory_msgs/MultiDOFJointTrajectoryPoint[] points
而且后者又涵盖了三个子消息类型:geometry_msgs/Transform[] transforms
、geometry_msgs/Twist[] velocities
和geometry_msgs/Twist[] accelerations
主要对无人机进行控制。
在对位置进行控制的时候,首先对x,y,z三个坐标进行数据获取,之后定义好yaw角度,也就是俯仰角(对无人机不进行角度的控制,只是单纯的位置控制)。
我这里想说明的是,在运行控制无人机的结点之前,需要在Gazebo中建立无人机模型以及控制方案,在之前机械臂控制中就需要ros_control的控制方案的选择,那么接下来对运行无人机模型的launch文件进行描述。
<launch> <arg name="mav_name" default="firefly"/> <arg name="world_name" default="outdoor"/> <arg name="enable_logging" default="false" /> <arg name="enable_ground_truth" default="true" /> <arg name="log_file" default="$(arg mav_name)" /> <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="true"/> --> <arg name="paused" value="true"/> <!-- <arg name="gui" value="false"/> --> <!-- <arg name="verbose" value="true"/> --> </include> <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_with_vi_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="hovering_example" pkg="rotors_gazebo" type="hovering_example" output="screen"/> <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="rviz" pkg="rviz" type="rviz" required="true" args="-d $(find rotors_gazebo)/$(arg mav_name).rviz"/> </group> </launch>
前面一些对变量的定义和目标文件位置的定义进行跳过,第一个运行的文件是gazebo_ros软件包下运行设定好的world文件的launch可执行launch文件,目的是将环境带入一些现实生活中的物理模型。接着第二个执行文件就是通过spawn_mav.launch对无人机模型进行导入。接着,也就是最重要的一部,lee_position_controller_node可执行文件就是对无人机的6个电机进行速度的控制,完成位置控制。接下来的三个结点就是发布一些自身无人机的数据,而rviz就是将无人机模型及其自身的摄像头进行订阅和显示。
最后放上一个演示效果图和演示视频,某人说放上EXO照片会长粉也不知道考不靠谱。
可是看到这高糊的图片,注定与爱豆EXO无缘了。
最后就放上正常的控制仿真视频的图片了:
Copyright © 2003-2013 www.wpsshop.cn 版权所有,并保留所有权利。