测试获取雷达数据:
启动顺序如下否则获取雷达数据失真
rosrun teleop_twist_keyboard teleop_twist_keyboard.py
roslaunch hector_quadrotor_demo gimbal_empty_world_test_flight_gazebo.launch
rosrun quadrotor_control_for_fengdian SubLaser
rosservice call /enable_motors true
使用c++控制无人机
先用c++实现键盘控制为后无需做准备(还在看):
实现盘控制
获取当前位置坐标
获取照片
任务1:到达风机的正前方
通过获取机器人雷达正前方距离,姿态,控制机器人。
先让无人机自旋转,获取最小的正前方的点,记录该点的姿态角,转玩一周后,在转到该角。
在实现时自转保持匀速,用时间记录自传一圈的时间
自旋转:
只设置z轴的的速度。
Twist.angular.z=0.1;//这里的角速度可以用PID改进其他啊x,y,z参数均为0;
控制turtlebot,实现机器人垂直面向最近的物体姿态角通过/odom 获取
主要问题:yaw角,相见4元祖到姿态角转换
turble仿真
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/PoseWithCovariance.h>
#include <std_msgs/Float64.h>
#include <sensor_msgs/LaserScan.h>
#include <nav_msgs/Odometry.h>
#include <math.h>
#include <ctime>
#define target_distance 6
#define PI 3.141592657
#define Ria_to_angle 180/3.141592627
using namespace std;
class Teleop
{
private:
ros::NodeHandle n_;//与机器人控制相关
ros::Publisher pub_;
ros::Subscriber sub_;
ros::NodeHandle n_laser;//与雷达信息相关
ros::Publisher pub_laser;
ros::Subscriber sub_laser;
std_msgs::Float64 Frontdata;
ros::NodeHandle n_odom;//与位置姿态相关
ros::Publisher pub_odom;
ros::Subscriber sub_odom;
float posedata[4];//雷达中间当前位置的姿态
float posedata_min[4];//雷达最小位置的姿态x,y,z,w
float agle_min_now_Z;
float laser_min=-1.0;//雷达的最小距离,设置-1为初始标志位,因为最小距离不可能有负值
int begin_time=-1.0;//自转一圈的开始时间
int spend_time=-1.0;//已经运动的时间
float self_angle_x=0;
float self_angle_y=0;
float self_angle_z=0;//最小值与当前值的x,y,z轴的旋转误差
public:
float agle_x=0,agle_y=0,agle_z=0;
float start_agle_x=-10;
int stop_flag=0;
float old_agle_x;
float now_x,now_y,now_z,min_z,min_x,min_y;
int big_flag=0;
Teleop(){
pub_=n_.advertise<geometry_msgs::Twist>("/cmd_vel_mux/input/teleop",1);
sub_=n_.subscribe("/cmd_vel_mux/input/teleop",10,&Teleop::callback,this);
pub_laser=n_laser.advertise<std_msgs::Float64>("/LaserFrontdata_topic", 1);
sub_laser=n_laser.subscribe("/scan", 1, &Teleop::subercallback, this);
pub_odom=n_odom.advertise<std_msgs::Float64>("/pose", 1);
sub_odom=n_odom.subscribe("/odom", 1, &Teleop::odomcallback, this);
}
void callback(const geometry_msgs::Twist& Twist);//调用control实现控制
void subercallback(const sensor_msgs::LaserScan::ConstPtr& msg);//读取雷达数据
void odomcallback(const nav_msgs::Odometry::ConstPtr& msg);//读取姿态
void control();//实现控制的主函数
void get_min();//利用历史中最小值与当前值进行比较,保留最小值
void get_time();//获取当前时间,和开始自转时间
void agle_min_now();//计算当前与最小之间的z轴姿态夹角
};
void Teleop::callback(const geometry_msgs::Twist& Twist){
//ROS_INFO("the linear x:%f,y:%f,z:%f",Twist.linear.x,Twist.linear.y,Twist.linear.z);
//ROS_INFO("the angular x:%f,y:%f,z:%f",Twist.angular.x,Twist.angular.y,Twist.angular.z);
Teleop::control();
}
//计算z,y,z轴姿态误差
void Teleop::agle_min_now()
{
old_agle_x=agle_x;
now_x=float(atan2(2*(posedata[2]*posedata[3]+posedata[0]*posedata[1]),(posedata[0]*posedata[0]-posedata[1]*posedata[1]-posedata[2]*posedata[2]+posedata[3]*posedata[3])));
now_y=float(asin(-2*(posedata[1]*posedata[3]-posedata[0]*posedata[2])));
now_z=float(atan2(2*(posedata[1]*posedata[2]+posedata[0]*posedata[3]),(posedata[0]*posedata[0]+posedata[1]*posedata[1]-posedata[2]*posedata[2]-posedata[3]*posedata[3])));
min_x=float(atan2(2*(posedata_min[2]*posedata_min[3]+posedata_min[0]*posedata_min[1]),(posedata_min[0]*posedata_min[0]-posedata_min[1]*posedata_min[1]-posedata_min[2]*posedata_min[2]+posedata_min[3]*posedata_min[3])));
min_y=float(asin(-2*(posedata_min[1]*posedata_min[3]-posedata_min[0]*posedata_min[2])));
min_z=float(atan2(2*(posedata_min[1]*posedata_min[2]+posedata_min[0]*posedata_min[3]),(posedata_min[0]*posedata_min[0]+posedata_min[1]*posedata_min[1]-posedata_min[2]*posedata_min[2]-posedata_min[3]*posedata_min[3])));
agle_x=now_x;
agle_y=now_y;
agle_z=now_z;
if(start_agle_x==-10)
{start_agle_x=agle_x;}
//agle_min_now_Z=now_z;
self_angle_x=agle_x-min_x;
self_angle_y=agle_y-min_y;
self_angle_z=agle_z-min_z;
};
//读取雷达正前方距离
void Teleop::subercallback(const sensor_msgs::LaserScan::ConstPtr& msg)
{
int mid;
mid =(msg->ranges).size()/2;
//ros::Publisher FrontData_Topic=n.advertise<std_msgs::Float64>("LaserFrontData", 1);
ROS_INFO("Front laserdata\n");
Frontdata.data=msg->ranges[mid];
ROS_INFO("%f\n",Frontdata.data);
pub_laser.publish(Frontdata);
//FrontData_Topic.publish(Frontdata);
}
//返回当前姿态x,y,z,w
void Teleop::odomcallback(const nav_msgs::Odometry::ConstPtr& msg)
{
posedata[0]=(msg->pose).pose.orientation.x;
posedata[1]=(msg->pose).pose.orientation.y;
posedata[2]=(msg->pose).pose.orientation.z;
posedata[3]=(msg->pose).pose.orientation.w;
//pub_odom.publish(posedata);
//ROS_INFO("pose x:\n");
}
//控制键盘自传
void Teleop::control(){
float vel=0.08*PI;//自选转速度单位rad/s
geometry_msgs::Twist Twist;
// 基于当前系统的当前日期/时间
//Twist.angular.z=0.1;
if(spend_time-2*PI/vel<=0.5)//转弯一圈后自动停止
{
Twist.angular.z=vel;
Teleop::agle_min_now(); //获得x,y,z的旋转角度
}
if(!(spend_time-2*PI/vel<=0.5))//已经转玩1圈
{
Teleop::agle_min_now(); //获得x,y,z的旋转角度
ROS_INFO("self_angle_x%f,self_angle_y%f,self_angle_z%f",self_angle_x,self_angle_y,self_angle_z);
if((abs(self_angle_x)>=0.01))//这里因为是自转只考虑z轴,旋转回垂直角
{
Twist.angular.z=self_angle_x/5;
if(abs(self_angle_x)<4)
{
if(self_angle_x>=0)
{Twist.angular.z=-0.02*(abs(self_angle_x)+3);}//
if(self_angle_x<0)
{Twist.angular.z=0.02*(abs(self_angle_x)+3);}
}
}
if(abs(self_angle_x)<=0.01)
{
if(target_distance-Frontdata.data!=0)
{Twist.linear.x=0.01*(target_distance-Frontdata.data)/50+5;}
}
}
pub_.publish(Twist);
Teleop::get_min();//比较距离获取最小值
//ROS_INFO("set vel %f,Frontdata:%f,min_Frontdata:%f,pose.x:%f,pose.y:%f,pose.z:%f,pose.w:%f\n",Twist.angular.z,Frontdata.data,laser_min,posedata[0],posedata[1],posedata[2],posedata[3]);
//ROS_INFO("minpose.x:%f,minpose.y:%f,minpose.z:%f,minpose.w:%f\n",posedata_min[0],posedata_min[1],posedata_min[2],posedata_min[3]);
if(old_agle_x!=agle_x)
{
//ROS_INFO("old_agle_x:%f,start_agle_x:%f,agle_x:%f,agle_y:%f,agle_z:%f,begin_time%d,spend_time%d",old_agle_x*Ria_to_angle,start_agle_x*Ria_to_angle,agle_x*Ria_to_angle,agle_y,agle_z,begin_time,spend_time);
ROS_INFO("frontdata%f,laser_min%f,min_x:%f,min_y:%f,min_z:%f,now_x%f,now_y%f,now_z%f",Frontdata.data,laser_min,min_x,min_y,min_z,now_x,now_y,now_z);
}
Teleop::get_time();
//if((old_agle_x<start_agle_x)&&(agle_x>=start_agle_x))
// {stop_flag=1;}
//if(stop_flag==1)
// {Twist.angular.z=0;}
//}
}
//获取最小距离
//Frontdata 雷达正前方距离
//laser_min 雷达正前方最小距离
void Teleop::get_min()
{
if(((laser_min==-1.0)||(laser_min>Frontdata.data))&&(Frontdata.data>0))
{laser_min=Frontdata.data;
memcpy(posedata_min, posedata, 4 * sizeof(float));//获取最小距离的姿态
}
}
//获得开始时间,最小值当前时间单位S
void Teleop::get_time()
{
time_t now = time(0);
//cout << "1970 到目前经过秒数:" << now << endl;
if(begin_time<0)
{begin_time=now;}
else
{spend_time=now-begin_time;}
//tm *ltm = localtime(&now);
// 输出 tm 结构的各个组成部分
// cout << "年: "<< 1900 + ltm->tm_year << endl;
// cout << "月: "<< 1 + ltm->tm_mon<< endl;
//cout << "日: "<< ltm->tm_mday << endl;
//cout << "时间: "<< ltm->tm_hour << ":";
//cout << ltm->tm_min << ":";
//cout << ltm->tm_sec << endl;
//if(begin_time==-1)//获得开始时间
// {begin_time=ros::Time::now();}
}
int main(int argc,char** argv){
ros::init(argc,argv,"teleop_test");
Teleop teleop;
ros::spin();
return 0;
}
验证 :已通过
使用hector仿真,问题偶尔风机容易飞天,测不到雷达数据
实现原理如上,这里的姿态获取的通过/UAV_StateInfo_msg 直接获取
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/PoseWithCovariance.h>
#include <std_msgs/Float64.h>
#include <sensor_msgs/LaserScan.h>
#include <nav_msgs/Odometry.h>
#include <math.h>
#include <ctime>
#include <simple/UAV_StateInfo.h>
#define target_distance 6
#define PI 3.141592657
#define Ria_to_angle 180/3.141592627
#define angle_to_ria 3.141592627/180
using namespace std;
class Teleop
{
private:
ros::NodeHandle n_;//与机器人控制相关
ros::Publisher pub_;
ros::Subscriber sub_;
ros::NodeHandle n_laser;//与雷达信息相关
ros::Publisher pub_laser;
ros::Subscriber sub_laser;
std_msgs::Float64 Frontdata;
ros::NodeHandle n_odom;//与位置姿态相关
ros::Publisher pub_odom;
ros::Subscriber sub_odom;
float posedata[3];//雷达中间当前位置的姿态角
float posedata_min[3];//雷达最小位置的姿态角
//float agle_min_now_Z;
float laser_min=-1.0;//雷达的最小距离,设置-1为初始标志位,因为最小距离不可能有负值
int begin_time=-1.0;//自转一圈的开始时间
int spend_time=-1.0;//已经运动的时间
float self_roll=0;
float self_yaw=0;
float self_pitch=0;//最小值与当前值的x,y,z轴的旋转误差
public:
float agle_x=0,agle_y=0,agle_z=0;
float start_agle_x=-10;
int stop_flag=0;
float old_agle_x;
float now_x,now_y,now_z,min_z,min_x,min_y;
int big_flag=0;
Teleop(){
pub_=n_.advertise<geometry_msgs::Twist>("/cmd_vel",10);
sub_=n_.subscribe("/cmd_vel",10,&Teleop::callback,this);
pub_laser=n_laser.advertise<std_msgs::Float64>("/LaserFrontdata_topic", 1);
sub_laser=n_laser.subscribe("/fengdian/scan", 1, &Teleop::subercallback, this);
//pub_odom=n_odom.advertise<std_msgs::Float64>("/pose", 1);
sub_odom=n_odom.subscribe("/UAV_StateInfo_msg", 1, &Teleop::odomcallback, this);
}
void callback(const geometry_msgs::Twist& Twist);//调用control实现控制
void subercallback(const sensor_msgs::LaserScan::ConstPtr& msg);//读取雷达数据
void odomcallback(const simple::UAV_StateInfo::ConstPtr& msg);//读取姿态
void control();//实现控制的主函数
void get_min();//利用历史中最小值与当前值进行比较,保留最小值
void get_time();//获取当前时间,和开始自转时间
//void agle_min_now();//计算当前与最小之间的z轴姿态夹角
};
void Teleop::callback(const geometry_msgs::Twist& Twist){
//ROS_INFO("the linear x:%f,y:%f,z:%f",Twist.linear.x,Twist.linear.y,Twist.linear.z);
//ROS_INFO("the angular x:%f,y:%f,z:%f",Twist.angular.x,Twist.angular.y,Twist.angular.z);
}
//计算z,y,z轴姿态误差
//void Teleop::agle_min_now()
//{
//
// old_agle_x=agle_x;
// now_x=float(atan2(2*(posedata[2]*posedata[3]+posedata[0]*posedata[1]),(posedata[0]*posedata[0]-posedata[1]*posedata[1]-posedata[2]*posedata/[2]+posedata[3]*posedata[3])));
// now_y=float(asin(-2*(posedata[1]*posedata[3]-posedata[0]*posedata[2])));
// now_z=float(atan2(2*(posedata[1]*posedata[2]+posedata[0]*posedata[3]),(posedata[0]*posedata[0]+posedata[1]*posedata[1]-posedata[2]*posedata[2]-posedata[3]*posedata[3])));
// min_x=float(atan2(2*(posedata_min[2]*posedata_min[3]+posedata_min[0]*posedata_min[1]),(posedata_min[0]*posedata_min[0]-posedata_min[1]*posedata_min[1]-posedata_min[2]*posedata_min[2]+posedata_min[3]*posedata_min[3])));
// min_y=float(asin(-2*(posedata_min[1]*posedata_min[3]-posedata_min[0]*posedata_min[2])));
// min_z=float(atan2(2*(posedata_min[1]*posedata_min[2]+posedata_min[0]*posedata_min[3]),(posedata_min[0]*posedata_min[0]+posedata_min[1]*posedata_min[1]-posedata_min[2]*posedata_min[2]-posedata_min[3]*posedata_min[3])));
// agle_x=now_x;
// agle_y=now_y;
// agle_z=now_z;
// if(start_agle_x==-10)
// {start_agle_x=agle_x;}
//agle_min_now_Z=now_z;
// self_angle_x=agle_x-min_x;
// self_angle_y=agle_y-min_y;
// self_angle_z=agle_z-min_z;
//};
//读取雷达正前方距离
void Teleop::subercallback(const sensor_msgs::LaserScan::ConstPtr& msg)
{
int mid;
mid =(msg->ranges).size()/2;
//ros::Publisher FrontData_Topic=n.advertise<std_msgs::Float64>("LaserFrontData", 1);
ROS_INFO("Front laserdata\n");
Frontdata.data=msg->ranges[mid];
ROS_INFO("%f\n",Frontdata.data);
Teleop::control();
pub_laser.publish(Frontdata);
//FrontData_Topic.publish(Frontdata);
}
//返回当前姿态x,y,z,w
void Teleop::odomcallback(const simple::UAV_StateInfo::ConstPtr& msg)
{
posedata[0]=(msg->Pitch);
posedata[1]=(msg->Roll);
posedata[2]=(msg->Yaw);
//posedata[3]=(msg->pose).pose.orientation.w;
//pub_odom.publish(posedata);
//ROS_INFO("pose x:\n");
}
//控制键盘自传
void Teleop::control(){
float vel=0.02*PI;//自选转速度单位rad/s
geometry_msgs::Twist Twist;
Twist.linear.x=0;Twist.linear.y=0;Twist.linear.z=0;
Twist.angular.x=0;Twist.angular.y=0;Twist.angular.z=0;
if(spend_time<=3)
{
Twist.linear.z=0.1*PI;
}
// 基于当前系统的当前日期/时间
//Twist.angular.z=0.1;
//if(spend_time-2*PI/vel<=0.5)//转弯一圈后自动停止
// {
if((spend_time>3)&&(spend_time<=2*PI/vel+3+1))
{
Twist.angular.z=vel;
}
Teleop::get_min();//比较距离获取最小
// }
if(spend_time>2*PI/vel+3+1)//已经转玩1圈
{
self_pitch=posedata[0]-posedata_min[0];
self_roll=posedata[1]-posedata_min[1];
self_yaw=posedata[2]-posedata_min[2];
if((abs(self_yaw)>=0.09))//这里因为是自转只考虑z轴yaw,旋转回垂直角
{
Twist.angular.z=angle_to_ria*self_yaw/5;
if(abs(self_yaw)>170)
{
if(self_yaw>=0)
{Twist.angular.z=-0.02*PI;}//
if(self_yaw<0)
{Twist.angular.z=0.02*PI;}
}
if(abs(self_yaw)<20)
{
if(self_yaw>=0)
{Twist.angular.z=-0.02*PI*0.02*(abs(self_yaw)+10);}//
if(self_yaw<0)
{Twist.angular.z=0.02*PI*0.02*(abs(self_yaw)+10);}
}
}
//if((abs(target_distance-Frontdata.data))!=0)
// {
// if(target_distance-Frontdata.data!=0)
// {Twist.linear.x=-angle_to_ria*0.01*(target_distance-Frontdata.data)/50+1;}
//
// }
}
ROS_INFO("Pitch:%f, Roll:%f, Yaw:%f,self_yaw%f,target_yaw%f,laser_min%f,frontdata%f,\n",posedata[0],posedata[1],posedata[2],self_yaw,posedata_min[2],laser_min,Frontdata.data);
pub_.publish(Twist);
//}
Teleop::get_time();
ROS_INFO("spendtime:%ds",spend_time);
//if((old_agle_x<start_agle_x)&&(agle_x>=start_agle_x))
// {stop_flag=1;}
//if(stop_flag==1)
// {Twist.angular.z=0;}
//}
}
//获取最小距离
//Frontdata 雷达正前方距离
//laser_min 雷达正前方最小距离
void Teleop::get_min()
{
if(((laser_min==-1.0)||(laser_min>Frontdata.data))&&(Frontdata.data>0))
{laser_min=Frontdata.data;
memcpy(posedata_min, posedata, 3 * sizeof(float));//获取最小距离的姿态
}
}
//获得开始时间,最小值当前时间单位S
void Teleop::get_time()
{
time_t now = time(0);
//cout << "1970 到目前经过秒数:" << now << endl;
if(begin_time<0)
{begin_time=now;}
else
{spend_time=now-begin_time;}
//tm *ltm = localtime(&now);
// 输出 tm 结构的各个组成部分
// cout << "年: "<< 1900 + ltm->tm_year << endl;
// cout << "月: "<< 1 + ltm->tm_mon<< endl;
//cout << "日: "<< ltm->tm_mday << endl;
//cout << "时间: "<< ltm->tm_hour << ":";
//cout << ltm->tm_min << ":";
//cout << ltm->tm_sec << endl;
//if(begin_time==-1)//获得开始时间
// {begin_time=ros::Time::now();}
}
int main(int argc,char** argv){
ros::init(argc,argv,"teleop_fly");
Teleop teleop;
ros::spin();
return 0;
}
验证 :风机上天的时候不行,正常已通过