move_base做路径规划时，利用程序设置机器人在RVIZ环境下的初始位置坐标。

admin管理员组

文章数量:1660165

在rviz仿真环境下，可以通过2D Pose Estimate实现车辆或机器人的初始位置，实际车辆或机器人不在rviz仿真环境下，如何通过程序指定机器人的初始位姿呢？

2D Pose Estimate

2D位姿估计（2D pose estimate) 是允许用户通过设定机器人在实际环境中的位姿，初始化导航功能包集的定位系统。
导航功能包集会等待名为initialpose的新主题的初始化位姿消息，这个主题是通过rviz窗口发布的。
在rviz窗口中，单击2D pose estimate 按钮，并单击地图来指定机器人的初始位姿。如果你一开始不进行这一项工作，机器人会启动一个自动定位进程来设定初始位姿。

Topic: initialpose
Type: geometry_msgs/PoseWithCovarianceStamped

geometry_msgs/PoseWithCovarianceStamped

//Compact Message Definition
geometry_msgs/Pose pose
float64[36] covariance

//1)geometry_msgs/Pose.msg
geometry_msgs/Point position 
//This contains the position of a point in free space
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation
//This represents an orientation in free space in quaternion form.
float64 x
float64 y
float64 z
float64 w

//(2
float64[36] covariance
//Row-major representation of the 6x6 covariance matrix
//The orientation parameters use a fixed-axis representation.
//In order, the parameters are:
//(x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)

创建CPP文件，编写节点发布程序

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include "cmath"

int main(int argc, char **argv)
{
  ros::init(argc, argv, "pose_estimate_2d");
  ros::NodeHandle nh;
  ros::Publisher initial_pose_pub = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("initialpose", 10);
  ros::Rate loop_rate(5);
  //define 2d estimate pose
  double alpha = M_PI/4;//radian value
  double x_pos = 10.0;
  double y_pos = 10.0;
 
  while (ros::ok())
  {
    geometry_msgs::PoseWithCovarianceStamped pose_msg;
 
    pose_msg.header.stamp = ros::Time::now();
    //地图坐标系
    pose_msg.header.frame_id = "map";
    pose_msg.pose.pose.position.x = x_pos;
    pose_msg.pose.pose.position.y = y_pos;
    //协方差矩阵，可根据实际需要，调整参数。
    pose_msg.pose.covariance[0] = 0.25;
    pose_msg.pose.covariance[6 * 1 + 1] = 0.25;
    pose_msg.pose.covariance[6 * 5 + 5] = 0.06853891945200942;
    pose_msg.pose.pose.orientation.z = sin(alpha);
    pose_msg.pose.pose.orientation.w = cos(alpha);
 
    initial_pose_pub.publish(pose_msg);
    ROS_INFO("Setting to :(%f,%f)",x_pos,y_pos);
    // 其实每次在通过2D pose estimate 指令点击RVIZ中的栅格地图时，在终端处也会出现同样的位置消息，即用C++程序取代RVIZ环境下的人工选取点。
    ros::spinOnce();
 
    loop_rate.sleep();
  }
 
  return 0;
}

通过rosrun命令就可以启动该节点，从而实现机器人初始位姿的设置。
欢迎交流！

本文标签： 坐标机器人路径位置环境