ROS获取地面站上传的航点信息进行offboard控制

最新推荐文章于 2024-05-21 10:54:31 发布

南京超维空间智能科技有限公司

最新推荐文章于 2024-05-21 10:54:31 发布

阅读量100

点赞数 2

分类专栏： VIP和客户文章标签： ROS 无人机

本文链接：https://blog.csdn.net/qq_35598561/article/details/138342355

版权

VIP和客户专栏收录该内容

2 篇文章 0 订阅

订阅专栏

一、订阅航点话题数据

 ros::Subscriber waypoint_sub = nh.subscribe<mavros_msgs::WaypointList>("/mavros/mission/waypoints", 100, waypoints_cb);

二、航点信息回调函数

mavros_msgs::WaypointList waypoints;
bool flag_waypoints_receive = false;
void waypoints_cb(const mavros_msgs::WaypointList::ConstPtr& msg)
{
	flag_waypoints_receive = true;
	waypoints = *msg;
}

三、消息结构体，主要获取经纬度和高

== uint16 command //可以根据command判断类型，比如是航点还是返航点或者起飞点等等==

[mavros_msgs/WaypointList]:
uint16 current_seq
mavros_msgs/Waypoint[] waypoints
  uint8 FRAME_GLOBAL=0
  uint8 FRAME_LOCAL_NED=1
  uint8 FRAME_MISSION=2
  uint8 FRAME_GLOBAL_REL_ALT=3
  uint8 FRAME_LOCAL_ENU=4
  uint8 frame
  uint16 command   //可以可以command判断类型
  bool is_current
  bool autocontinue
  float32 param1
  float32 param2
  float32 param3
  float32 param4
  float64 x_lat    //经
  float64 y_long   //纬
  float64 z_alt    //高

[yocs_msgs/WaypointList]:
yocs_msgs/Waypoint[] waypoints
  std_msgs/Header header
    uint32 seq
    time stamp
    string frame_id
  string name
  geometry_msgs/Pose pose
    geometry_msgs/Point position
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion orientation
      float64 x
      float64 y
      float64 z
      float64 w

#include <ros/ros.h>
#include <Eigen/Eigen>
#include <cmath>
#include <algorithm>
#include <vector>
#include <iostream>
#include <std_msgs/Bool.h>
#include <mavros_msgs/State.h>
#include <geometry_msgs/Pose.h>
#include <sensor_msgs/LaserScan.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/PositionTarget.h>
#include <eigen_conversions/eigen_msg.h>
#include <mavros/frame_tf.h>
#include <mavros_msgs/WaypointList.h>
#include <std_msgs/Float64.h>
#include <mavros_msgs/HomePosition.h>
#include <GeographicLib/Geocentric.hpp>
#include <sensor_msgs/NavSatFix.h>
#include <tf/transform_listener.h>
#include <nav_msgs/Odometry.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/CommandLong.h>   


using namespace std;

//#define ALTITUDE 1.0

float ALTITUDE=2.0;

std::vector<geometry_msgs::PoseStamped> pose;
mavros_msgs::PositionTarget pos_target;


mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg) 
{
	current_state = *msg;
}

mavros_msgs::HomePosition home_pos;
void home_pos_cb(const mavros_msgs::HomePosition::ConstPtr& msg) 
{
	home_pos = *msg;
}

geometry_msgs::PoseStamped local_pos;
Eigen::Vector3d current_local_pos;


//定义变量，用于接收无人机的里程计信息
tf::Quaternion quat; 
double roll, pitch, yaw;
float init_position_x_take_off =0;
float init_position_y_take_off =0;
float init_position_z_take_off =0;
bool  flag_init_position = false;
nav_msgs::Odometry local_odom;
void local_odom_cb(const nav_msgs::Odometry::ConstPtr& msg)
{
    local_odom = *msg;
    if (flag_init_position==false && (local_odom.pose.pose.position.z!=0))
    {
		init_position_x_take_off = local_odom.pose.pose.position.x;
	    init_position_y_take_off = local_odom.pose.pose.position.y;
	    init_position_z_take_off = local_odom.pose.pose.position.z;
        flag_init_position = true;		    
    }
    tf::quaternionMsgToTF(local_odom.pose.pose.orientation, quat);	
	tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);
}
void local_pos_cb(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
	current_local_pos = mavros::ftf::to_eigen(msg->pose.position);
	local_pos = *msg;
}

mavros_msgs::WaypointList waypoints;
bool flag_waypoints_receive = false;
void waypoints_cb(const mavros_msgs::WaypointList::ConstPtr& msg)
{
	flag_waypoints_receive = true;
	waypoints = *msg;
}


Eigen::Vector3d current_gps;
void gps_cb(const sensor_msgs::NavSatFix::ConstPtr &msg)
{
	current_gps = { msg->latitude, msg->longitude, msg->altitude };
}

//8、USB二维摄像头只能给出目标在图像中的位置
geometry_msgs::PointStamped object_pos; 
double position_detec_x = 0;
double position_detec_y = 0;
double position_detec_z = 0;
void   object_pos_cb(const geometry_msgs::PointStamped::ConstPtr& msg);
void   object_pos_cb(const geometry_msgs::PointStamped::ConstPtr& msg)
{
	object_pos = *msg;
}

float object_recognize_track_vel_last_time_position_x = 0;
float object_recognize_track_vel_last_time_position_y = 0;
bool  object_recognize_track_vel_flag = false;
bool  object_recognize_track_vel(string str, float yaw, float ALTITUDE, float speed, float error_max);
bool  object_recognize_track_vel(string str, float yaw, float ALTITUDE, float speed, float error_max)
{
	if(object_recognize_track_vel_flag == false)
	{
		object_recognize_track_vel_last_time_position_x = local_odom.pose.pose.position.x;
    	object_recognize_track_vel_last_time_position_y = local_odom.pose.pose.position.y;
		object_recognize_track_vel_flag = true;
	}
	if(object_pos.header.frame_id == str)
    {
        object_recognize_track_vel_last_time_position_x = local_odom.pose.pose.position.x;
        object_recognize_track_vel_last_time_position_y = local_odom.pose.pose.position.y;

        position_detec_x = object_pos.point.x;
        position_detec_y = object_pos.point.y;
        position_detec_z = object_pos.point.z; 
       // ROS_INFO("position_detec_x  = %f\r",    position_detec_x);
       // ROS_INFO("position_detec_y  = %f\r",    position_detec_y);
        //ROS_INFO("position_detec_z  = %f\r\n",  position_detec_z);

		if(fabs(position_detec_x-320) < error_max && fabs(position_detec_y-240) < error_max)
		{
			ROS_INFO("到达目标的正上/前方，在允许误差范围内保持");
			return true;
		}
		else
		{
			if(position_detec_x -320 >= error_max)
			{
				pos_target.velocity.y =  -speed;
			}		
			else if(position_detec_x - 320 <= -error_max)
			{
				pos_target.velocity.y =  speed;
			}	
			else
			{
				pos_target.velocity.y =  0;
			}					  
			if(position_detec_y -240 >= error_max)
			{
				pos_target.velocity.x =  -speed;
			}
			else if(position_detec_y - 240 <= -error_max)
			{
				pos_target.velocity.x =  speed;
			}
			else
			{
				pos_target.velocity.x =  0;
			}			
       		pos_target.type_mask = 1 + 2 +/* 4 + 8 + 16 + 32*/ + 64 + 128 + 256 + 512 + 1024 + 2048;
	 		pos_target.coordinate_frame = 8;
			pos_target.position.z = init_position_z_take_off+ALTITUDE;
		}
	}
	else
	{	
		pos_target.position.x = object_recognize_track_vel_last_time_position_x;
		pos_target.position.y = object_recognize_track_vel_last_time_position_y;
		pos_target.type_mask = /*1 + 2 + 4 + 8 + 16 + 32*/ + 64 + 128 + 256 + 512 /*+ 1024 + 2048*/;
		pos_target.coordinate_frame = 1;
		pos_target.position.z = init_position_z_take_off + ALTITUDE;
		pos_target.yaw        = yaw;
	}
	return false;
}

ros::Time lib_mission_success_time_record;
bool lib_time_record_start_flag = false;
bool lib_time_record_func(float time_duration,ros::Time time_now);
bool lib_time_record_func(float time_duration,ros::Time time_now)
{
	if(lib_time_record_start_flag == false)
	{
		lib_mission_success_time_record = time_now;
		lib_time_record_start_flag = true;
	}
	if(ros::Time::now() -lib_mission_success_time_record > ros::Duration(time_duration))
	{
		lib_time_record_start_flag = false;
		return true;
	}
	else
	{
		return false;
	}
}

mavros_msgs::CommandLong lib_ctrl_pwm;
void lib_pwm_control(int pwm_channel_5, int pwm_channel_6);
void lib_pwm_control(int pwm_channel_5, int pwm_channel_6)
{
     lib_ctrl_pwm.request.command = 187;    
     lib_ctrl_pwm.request.param1 = ((pwm_channel_5/50.0)-1);        
     lib_ctrl_pwm.request.param2 = ((pwm_channel_6/50.0)-1);     
}




//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int main(int argc, char **argv)
{
    ros::init(argc, argv, "qgc_mission");
    ros::NodeHandle nh;

    ros::Subscriber gps_sub = nh.subscribe("/mavros/global_position/global",100,gps_cb);
    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>("/mavros/state", 100, state_cb);
    ros::Subscriber local_pos_sub = nh.subscribe<geometry_msgs::PoseStamped>("/mavros/local_position/pose", 100, local_pos_cb);
    ros::Subscriber local_odom_sub = nh.subscribe<nav_msgs::Odometry>("/mavros/local_position/odom", 10, local_odom_cb);
    ros::Subscriber waypoint_sub = nh.subscribe<mavros_msgs::WaypointList>("/mavros/mission/waypoints", 100, waypoints_cb);
    ros::Subscriber homePos_sub = nh.subscribe<mavros_msgs::HomePosition>("/mavros/home_position/home", 100, home_pos_cb);
	ros::Publisher local_pos_pub = nh.advertise<mavros_msgs::PositionTarget>("/mavros/setpoint_raw/local", 100);

	ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");
    ros::ServiceClient ctrl_pwm_client = nh.serviceClient<mavros_msgs::CommandLong>("mavros/cmd/command");
    ros::Rate rate(20.0);
	while(ros::ok() && !current_state.connected) 
	{
		ros::spinOnce();
		rate.sleep();
	}

	//此处wihle等待上传航点，没有航点的话则一直等待
	while (ros::ok())
	{
		for(int index = 0; index < waypoints.waypoints.size(); index++)
		{
			if(waypoints.waypoints[index].command==20)
			{
				
			}
			else
			{
				geometry_msgs::PoseStamped p;
				GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());
				Eigen::Vector3d goal_gps(waypoints.waypoints[index].x_lat, waypoints.waypoints[index].y_long, 0);
				Eigen::Vector3d current_ecef;
				earth.Forward(current_gps.x(), current_gps.y(), current_gps.z(),
				current_ecef.x(), current_ecef.y(), current_ecef.z());
				Eigen::Vector3d goal_ecef;
				earth.Forward(goal_gps.x(), goal_gps.y(), goal_gps.z(),
				goal_ecef.x(), goal_ecef.y(), goal_ecef.z());
				Eigen::Vector3d ecef_offset = goal_ecef - current_ecef;
				Eigen::Vector3d enu_offset = mavros::ftf::transform_frame_ecef_enu(ecef_offset, current_gps);
				Eigen::Affine3d sp;
				Eigen::Quaterniond q;
				q = Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX())
				* Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY())
				* Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitZ());
				sp.translation() = current_local_pos + enu_offset;
				sp.linear() = q.toRotationMatrix();
				//*******************************往vector容器存数据****************************************
				Eigen::Vector3d testv(sp.translation());
				p.pose.position.x = testv[0];
				p.pose.position.y = testv[1];
				p.pose.position.z = waypoints.waypoints[index].z_alt;
				pose.push_back(p);
				printf("pose.size() = %d\r\n", pose.size());
				printf("command = %d\r\n", waypoints.waypoints[index].command);
				
				printf("p.pose.position.x = %f\r\n", p.pose.position.x);
				printf("p.pose.position.y = %f\r\n", p.pose.position.y);
				printf("p.pose.position.z = %f\r\n", p.pose.position.z);
			}			
		}
		if(waypoints.waypoints.size()==0)
		{
			printf("Please set the waypoint in QGC before running this program.\n");
		}
		else
		{
			break;
		}
		ros::spinOnce();
		rate.sleep();
	}
	pos_target.coordinate_frame = 1;
	pos_target.type_mask = /*1 + 2 + 4*+ */8 + 16 + 32 + 64 + 128 + 256 + 512 + 1024 + 2048;
	pos_target.position.x = pose[0].pose.position.x;
	pos_target.position.y = pose[0].pose.position.y;
	pos_target.position.z = init_position_z_take_off + pose[0].pose.position.z;
	//pos_target.position.z = init_position_z_take_off + ALTITUDE;
	
	//send a few setpoints before starting
    for(int i = 100; ros::ok() && i > 0; --i)
    {
 		local_pos_pub.publish(pos_target);
        ros::spinOnce();
        rate.sleep();
    }
	
	mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";

    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;

    ros::Time last_request = ros::Time::now();
    
 	//此处满足一次请求进入offboard模式即可，官方例成循环切入offboard会导致无人机无法使用遥控器控制
    while(ros::ok())
    {
    	//请求进入OFFBOARD模式
        if( current_state.mode != "OFFBOARD" && (ros::Time::now() - last_request > ros::Duration(5.0)))
        {
            if( set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent)
            {
                ROS_INFO("Offboard enabled");
            }
           	last_request = ros::Time::now();
       	}
        else 
		{
			//请求解锁
			if( !current_state.armed && (ros::Time::now() - last_request > ros::Duration(5.0)))
			{
		        if( arming_client.call(arm_cmd) && arm_cmd.response.success)
		       	{
		            ROS_INFO("Vehicle armed");
		        }
		        	last_request = ros::Time::now();
			}
		}
	    
		if(ros::Time::now() - last_request > ros::Duration(10.0))
		{
			break;
		}
		//发布期望位置信息
		pos_target.coordinate_frame = 1;
		pos_target.type_mask = /*1 + 2 + 4*+ */8 + 16 + 32 + 64 + 128 + 256 + 512 + 1024 + 2048;
		pos_target.position.x = pose[0].pose.position.x;
		pos_target.position.y = pose[0].pose.position.y;
		pos_target.position.z = init_position_z_take_off + ALTITUDE;
 		local_pos_pub.publish(pos_target);
        ros::spinOnce();
        rate.sleep();
    }   
	
	
		
	//循环跑航点，每到达一个航点位置，则输出一次arrive
	while (ros::ok())
	{
		for (int i = 0; i < pose.size(); i++)
		{
			while(ros::ok()) 
			{
				//判断是否看到目标
				if(object_pos.header.frame_id == "landing")
				{
					break;
				}
				if(fabs(local_pos.pose.position.x - pose[i].pose.position.x) < 1.0 && fabs(local_pos.pose.position.y - pose[i].pose.position.y) < 1.0)
				{
					printf("arrive\r\n");
					break;//当前位置与期望航点距离相差不到1m,则退出此次航点任务
				}				
				pos_target.coordinate_frame = 1;
				pos_target.type_mask = /*1 + 2 + 4*+ */8 + 16 + 32 + 64 + 128 + 256 + 512 + 1024 + 2048;
				pos_target.position.x = pose[i].pose.position.x;
				pos_target.position.y = pose[i].pose.position.y;
				pos_target.position.z = init_position_z_take_off + ALTITUDE;
 				local_pos_pub.publish(pos_target);
 				ros::spinOnce();
				rate.sleep();
			}
			//跑完所有航点后则跳出，准备进入其他的程序
			if((i+1)==pose.size())
			{
				mavros_msgs::SetMode offb_set_mode;
    			offb_set_mode.request.custom_mode = "AUTO.RTL";
    			if(set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent)
            	{
                	ROS_INFO("RTL enabled");
            	}
				printf("end\r\n");
				//break;
			}
		}
		while (ros::ok()) 
		{
			if(object_recognize_track_vel("landing", 0, ALTITUDE, 0.15, 40))
			{
				if(lib_time_record_func(2.0,ros::Time::now()))
				{
					lib_pwm_control(100,100);
					ctrl_pwm_client.call(lib_ctrl_pwm);
					ROS_INFO("舵机执行物体投放");
				}
			}
			local_pos_pub.publish(pos_target);
			ros::spinOnce();
			rate.sleep();
		}
		ros::spinOnce();
		rate.sleep();
	}
    return 0;
}