我是一个新的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 */
"""
1条答案
按热度按时间pxyaymoc1#
在BroadcastPath.cpp中,您应该在将pose推送到path之前为其指定一个头。您还应该将path的frame_id设置为parentId(如果使用的是ros标准,则为“odom”),并将pose的frame_id设置为childId(如果使用的是ros标准,则为“base_link”)。您的代码应如下所示: