【Gazebo无人机仿真】解决ROS+Gazebo+PX4中PositionTargetGlobal警告的方法

最近在参考博客“【ROS+Gazebo+px4】打开offboard模式，offboard_node文件内容”（https://blog.csdn.net/qq_51985653/article/details/130647051）使用ROS+Gazebo+PX4搭建无人机仿真环境时，遇到了一个小问题，即在运行节点rosrun offboard offboard_node
后，原运行roslaunch px4 mavros_posix_sitl.launch的控制台会出现PositionTargetGlobal failed because no origin的警告信息，虽然不影响无人机控制，但看着不太舒服。

问题分析

通过查看源码发现，警告信息是由mavros中的guided_target.cpp文件中的一段代码引发的：

my_ws/src/mavros/mavros_extras/src/plugins/guided_target.cpp中的		/* check origin has been set */
		if (!is_map_init) {
			ROS_WARN_NAMED("setpoint", "PositionTargetGlobal failed because no origin");
		}

具体而言，是由于is_map_init变量为false，导致打印警告信息。通过进一步查找，发现问题出在一个回调函数gp_origin_cb中：

void gp_origin_cb(const geographic_msgs::GeoPointStamped::ConstPtr &msg)
	{
		ecef_origin = {msg->position.latitude, msg->position.longitude, msg->position.altitude};
		/**
		 * @brief Conversion from ECEF (Earth-Centered, Earth-Fixed) to geodetic coordinates (LLA)
		*/
		GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());
		try {
			earth.Reverse(ecef_origin.x(), ecef_origin.y(), ecef_origin.z(),
				map_origin.x(), map_origin.y(), map_origin.z());
		}
		catch (const std::exception& e) {
			ROS_WARN_STREAM("setpoint: Caught exception: " << e.what() << std::endl);
			return;
		}
		is_map_init = true;
	}

它的subsriber的定义为

// Subscriber for global origin (aka map origin).
		gp_origin_sub = spg_nh.subscribe("global_position/gp_origin", 10, &GuidedTargetPlugin::gp_origin_cb, this);	

说明mavros节点没有接收到“global_position/gp_origin”（事实上是/mavros/global_position/gp_origin）的话题来初始化原点。这导致is_map_init一直保持为false，触发了警告信息的打印。

解决方法

为了解决该问题，可以通过自己编写一个节点，手动发布自定义的原点位置信息。以下是解决步骤：

第一步：创建发布原点信息的节点

在mavros的源码目录（my_ws/src/mavros/mavros/src/）下，创建一个名为pub_origin.cpp的文件，内容如下：

// pub_origin.cpp

#include <ros/ros.h>
#include <geographic_msgs/GeoPointStamped.h>

int main(int argc, char **argv)
{
    ros::init(argc, argv, "pub_origin");
    ros::NodeHandle nh("~");

    ros::Publisher gp_origin_pub = nh.advertise<geographic_msgs::GeoPointStamped>("/mavros/global_position/gp_origin", 10);

    ros::Rate rate(10.0);

    // Read parameters 
    double latitude, longitude, altitude;
    nh.param<double>("latitude", latitude, 0.0);
    nh.param<double>("longitude", longitude, 0.0);
    nh.param<double>("altitude", altitude, 0.0);

    geographic_msgs::GeoPointStamped gp_origin;
    gp_origin.header.stamp = ros::Time::now();
    gp_origin.header.frame_id = "map";
    gp_origin.position.latitude = latitude;
    gp_origin.position.longitude = longitude;
    gp_origin.position.altitude = altitude;

    while (ros::ok()) {
        gp_origin_pub.publish(gp_origin);
        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}

修改launch文件

在mavros的launch目录下找到node.launch文件，加入以下内容

<node pkg="mavros" type="pub_origin_node" name="pub_origin" output="$(arg log_output)">
    <param name="latitude" value="0.0" />
    <param name="longitude" value="0.0" />
    <param name="altitude" value="0.0" />
</node>

其中，三个参数可以根据实际情况修改。

第三步：修改CMakeLists.txt文件

在mavros的CMakeLists.txt文件中，加入以下内容：

add_executable(pub_origin_node src/pub_origin.cpp)
target_link_libraries(pub_origin_node mavros ${catkin_LIBRARIES})

第四步：编译

在工作空间根目录，即my_ws下执行编译命令catkin_make，确保mavros包能够被正确编译。

这样一来，在启动mavros时，将会自动启动一个节点用于发布原点位置信息，从而消除了警告信息的出现，提高使用体验。

希望这篇分享对大家在使用ROS+Gazebo+PX4搭建仿真环境时能够有所帮助。如果有任何疑问或建议，欢迎留言讨论。