c++ 无法在rviz ROS中显示发布路径

jjjwad0x  于 2023-02-26  发布在  其他
关注(0)|答案(1)|浏览(341)

我是一个新的ROS和尝试在ROS中发布一个路径。我已经检查了主题"/路径"发布,但路径似乎没有显示在rviz [我可以选择/路径,它似乎工作,但没有轨迹显示。我敢肯定,线是不透明的]。我不知道为什么?非常感谢,如果有人能帮助我。以下是我的代码:
BroadcastPath.cpp
"""

#include "control_turtle/BroadcastPath.hpp"

namespace broadcast_path {

BroadcastPath::BroadcastPath(ros::NodeHandle& nodeHandle)
    : nodeHandle_(nodeHandle)
{
    publisher_ = nodeHandle_.advertise<nav_msgs::Path>("/path",1);
    subscriber_ = nodeHandle_.subscribe("/odom", 2, &BroadcastPath::topicCallback, this);

}

BroadcastPath::~BroadcastPath()
{
}

void BroadcastPath::topicCallback(const nav_msgs::Odometry& msg)
{
    nav_msgs::Path path;
    geometry_msgs::PoseStamped pose;

    pose.pose.position = msg.pose.pose.position;
    pose.pose.orientation = msg.pose.pose.orientation;

    path.header.stamp = ros::Time::now();
    path.header.frame_id = "map";
    path.poses.push_back(pose);

    publisher_.publish(path);

}

} /* namespace */

"""
Broadcast_path_node.cpp
"""

#include <ros/ros.h>
#include "control_turtle/BroadcastPath.hpp"

int main(int argc, char** argv)
{
  ros::init(argc, argv, "broadcast_path");
  ros::NodeHandle nodeHandle("~");
  ros::Rate loopRate(10);

  broadcast_path::BroadcastPath broadcastPath(nodeHandle);

  ros::spin();
  return 0;
}

"""
BroadcastPath.hpp
"""

#pragma once

// ROS
#include <iostream>
#include <ros/ros.h>
#include <stdio.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>

namespace broadcast_path {

/*!
 * Main class for the node to handle the ROS interfacing.
 */
class BroadcastPath
{
 public:
  /*!
   * Constructor.
   * @param nodeHandle the ROS node handle.
   */
    BroadcastPath(ros::NodeHandle& nodeHandle);

  /*!
   * Destructor.
   */
  virtual ~BroadcastPath();

 private:

  /*!
   * ROS topic callback method.
   * @param message the received message.
   */
  void topicCallback(const nav_msgs::Odometry& msg);

  /*!
   * ROS service server callback.
   * @param request the request of the service.
   * @param response the provided response.
   * @return true if successful, false otherwise.
   */

  //! ROS node handle.
  ros::NodeHandle& nodeHandle_;

  //! ROS publisher
  ros::Publisher publisher_;
  ros::Subscriber subscriber_;

  int count = 0;

};

} /* namespace */

"""

pxyaymoc

pxyaymoc1#

在BroadcastPath.cpp中,您应该在将pose推送到path之前为其指定一个头。您还应该将path的frame_id设置为parentId(如果使用的是ros标准,则为“odom”),并将pose的frame_id设置为childId(如果使用的是ros标准,则为“base_link”)。您的代码应如下所示:

#include "control_turtle/BroadcastPath.hpp"

namespace broadcast_path {

BroadcastPath::BroadcastPath(ros::NodeHandle& nodeHandle)
    : nodeHandle_(nodeHandle)
{
    publisher_ = nodeHandle_.advertise<nav_msgs::Path>("/path",1);
    subscriber_ = nodeHandle_.subscribe("/odom", 2, &BroadcastPath::topicCallback, this);

}

BroadcastPath::~BroadcastPath()
{
}

void BroadcastPath::topicCallback(const nav_msgs::Odometry& msg)
{
    nav_msgs::Path path;
    geometry_msgs::PoseStamped pose;

    pose.header.stamp = ros::Time::now();
    path.header.frame_id = "odom";
    pose.pose.position = msg.pose.pose.position;
    pose.pose.orientation = msg.pose.pose.orientation;

    path.header.stamp = ros::Time::now();
    path.header.frame_id = "base_link";
    path.poses.push_back(pose);

    publisher_.publish(path);

}

} /* namespace */

相关问题