如何用xyz数据，让飞控在室内无gps情况下实现定位

shang

购买的p200产品，想试一下如何用uwb替代激光雷达定位，想自己先电脑模拟一下一组xyz的数据给飞控发去，让飞控处于定位状态，代码乳腺癌：#include <ros/ros.h>
#include <iostream>
#include <Eigen/Eigen>
//msg 头文件
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <std_msgs/Bool.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <geometry_msgs/Point.h>
#include <std_msgs/UInt16.h>
#include <std_msgs/Float64.h>
#include <tf2_msgs/TFMessage.h>
#include <geometry_msgs/TransformStamped.h>
#include <sensor_msgs/Range.h>

using namespace std;
float get_dt(ros::Time last);
geometry_msgs::PoseStamped vision;

//订阅mavros/state的回调函数
mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
    printf("message get callback!\n");
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "uwb");
    ros::NodeHandle nh;
   //订阅mavros/state
    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
            ("mavros/state", 10, state_cb);
    // 【发布】无人机位置和偏航角 坐标系 ENU系
    //  本话题要发送飞控(通过mavros_extras/src/plugins/vision_pose_estimate.cpp发送), 对应Mavlink消息为VISION_POSITION_ESTIMATE(#??), 对应的飞控中的uORB消息为vehicle_vision_position.msg 及 vehicle_vision_attitude.msg
    ros::Publisher vision_pub = nh.advertise<geometry_msgs::PoseStamped>("/mavros/vision_pose/pose", 100);

    // 频率
    ros::Rate rate(20.0);

//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>Main Loop<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
    while(ros::ok())
    {
        //回调一次 更新传感器状态
            ros::spinOnce();
            vision.pose.position.x = 116.3;
            vision.pose.position.y = 39.9;
            vision.pose.position.z = 3;

            vision.pose.orientation.x = 0.707107;
            vision.pose.orientation.y = 0;
            vision.pose.orientation.z =0;
            vision.pose.orientation.w = 0.707107;

        vision.header.stamp = ros::Time::now();
        vision_pub.publish(vision);

        //打印
        //printf_info();
        rate.sleep();
    }

    return 0;

}
//获取当前时间 单位：秒
float get_dt(ros::Time last)
{
    ros::Time time_now = ros::Time::now();
    float currTimeSec = time_now.sec-last.sec;
    float currTimenSec = time_now.nsec / 1e9 - last.nsec / 1e9;
    return (currTimeSec + currTimenSec);
}

实验了一下没有成功，请指点一下。

amov_msq

如果切入定点模式 可以切换成功 就表示这个位置数据可以用 你试试，但是你这个值感觉都好大

amov_msq

没有成功是什么反应

shang

没成功就是  运行这个节点后  打印了message get callback说明飞控连接上了   但是从qgc地面站看还是没有进入定位状态也

shang

我是想有一个demo可以给飞控发固定的一组经纬高坐标，让飞控进入定位状态。这样就方便后期替换这个定位数据是来自激光雷达还是摄像头或者超声波了

shang

就是发布出去的xyz数据，得把飞机切换到定点模式，才能看出来吗，这个我给的是经纬高