我想通过代码替代rviz中2D Pose Estimate功能。在RDK X3端运行以下节点。无法替代rviz中2D Pose Estimate功能。rviz中无反应,直接用鼠标点击rviz中2D Pose Estimate功能,是没问题的。
请问有什么办法实现吗?
以下代码无法正常初始化2D Pose Estimate功能
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
class PoseEstimatePublisherNode : public rclcpp::Node {
public:
PoseEstimatePublisherNode() : Node("pose_estimate_publisher_node") {
// 创建Publisher
pose_estimate_publisher_ = this->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("/initialpose", 1);
// 构建2D Pose Estimate消息
auto pose_estimate_msg = std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>();
pose_estimate_msg->header.stamp = this->now();
pose_estimate_msg->header.frame_id = "map"; // 设置坐标系
pose_estimate_msg->pose.pose.position.x = 0.27; // 设置机器人的x坐标
pose_estimate_msg->pose.pose.position.y = -0.44; // 设置机器人的y坐标
pose_estimate_msg->pose.pose.orientation.w = 1.0; // 设置机器人的朝向,假设朝向为0度
// 发布2D Pose Estimate消息
pose_estimate_publisher_->publish(*pose_estimate_msg);
RCLCPP_INFO(this->get_logger(), "Published initial pose estimate: x=%f, y=%f, orientation=%f",
pose_estimate_msg->pose.pose.position.x,
pose_estimate_msg->pose.pose.position.y,
pose_estimate_msg->pose.pose.orientation.w);
}
private:
rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_estimate_publisher_;
};
int main(int argc, char * argv[]) {
rclcpp::init(argc, argv);
auto node = std::make_shared<PoseEstimatePublisherNode>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
第三方账号登入
QQ 微博 微信