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命令就可以启动该节点,从而实现机器人初始位姿的设置。
欢迎交流!
版权声明:本文标题:move_base做路径规划时,利用程序设置机器人在RVIZ环境下的初始位置坐标。 内容由热心网友自发贡献,该文观点仅代表作者本人, 转载请联系作者并注明出处:https://m.elefans.com/dianzi/1729850792a1215381.html, 本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如发现本站有涉嫌抄袭侵权/违法违规的内容,一经查实,本站将立刻删除。
发表评论