如何在C++程序中与nav2交互

code .

已于 2024-01-09 17:50:54 修改

阅读量446

点赞数 11

分类专栏：项目记录 # Nav2 阿克曼小车导航 C++积累文章标签： c++ 交互开发语言自动驾驶 ros2 nav2

于 2023-12-24 18:46:51 首次发布

本文链接：https://blog.csdn.net/vmt/article/details/135184832

版权

本文介绍了如何在C++程序中与nav2进行交互，包括获取机器人坐标、初始化小车位置、加载地图、判断导航服务状态、发布导航目标及动态调整nav2参数的详细步骤。内容基于ROS2和自动驾驶环境。

摘要由CSDN通过智能技术生成

1 获取机器人坐标

有多个方法可以获得机器人在全局地图下的坐标，我采用查询TF的方法，得到base_footprint 到map的TF，从而得到机器人的x，y和yaw。

 geometry_msgs::msg::TransformStamped t;
    try {
   
        t = tf_buffer_->lookupTransform(
            "map", "base_footprint",
            tf2::TimePointZero);
    }
    catch (const tf2::TransformException & ex) {
   
        if(_bLocalised)RCLCPP_INFO(
            node->get_logger(), "Could not transform %s to %s: %s",
            "map", "base_footprint", ex.what());  
        _bLocalised=false;
        return;
    }
    tf2::Quaternion myQuaternion(t.transform.rotation.x,t.transform.rotation.y,t.transform.rotation.z,t.transform.rotation.w);
    tf2::Matrix3x3 m(myQuaternion);
    double yaw,roll,pitch;
    m.getEulerYPR(yaw,pitch,roll);
    emit posUpdate(true,t.transform.translation.x,t.transform.translation.y,yaw);
    current_pose_x=t.transform.translation.x;
    current_pose_y=t.transform.translation.y;
    current_pose_angle=yaw*180.0/M_PI;

2 初始化小车位置

通过发布当前位置消息（PoseWithCovarianceStamped 类型）到topic：“initialpose” 来让nav2进行位置进行初始化

::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr _initial_pose_pub;
   // -----------------
     _initial_pose_pub =node->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("initialpose", 10);
   // -----------------
void rclcomm::PublishInitpos(float fx, float fy, float fa)
{
   
     geometry_msgs::msg::PoseWithCovarianceStamped iniPosMsg;
     iniPosMsg.header.stamp = rclcpp::Clock().now();
     iniPosMsg.header.frame_id = "map";
     iniPosMsg.pose.pose.position.x = fx;
     iniPosMsg.pose.pose.position.y = fy;
     iniPosMsg.pose.covariance[0] = 0.25;
     iniPosMsg.pose.covariance[6 * 1 + 1] = 0.25;
     iniPosMsg.pose.covariance[6 * 5 + 5] = 0.06853891945200942;
     iniPosMsg.pose.pose.orientation.z = sin(fa*M_PI/180/2);
     iniPosMsg.pose.pose.orientation.w = cos(fa*M_PI/180/2);

    _initial_pose_pub->publish(iniPosMsg);
     std::cout