在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
geometry_msgs/Pose pose
float64[36] covariance
geometry_msgs/Point position
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
float64[36] covariance
创建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);
double alpha = M_PI/4;
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);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
通过rosrun命令就可以启动该节点,从而实现机器人初始位姿的设置。
欢迎交流!
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)