Which topic should I publish path to?
Hi guys,
I am trying to implement a global path planner to Turtlebot 2 using ROS navigation.
Now I can get the initial position, goal position and map data these three for path planning.
And I am trying to verify my output.
I wrote a simple TALKER to send path to topic /move_base/TrajectoryPlannerROS/global_plan
.
int main(int argc, char **argv)
{
ros::init(argc, argv, "talker");
ros::NodeHandle p;
ros::Publisher path_pub = p.advertise<nav_msgs::Path>("move_base/TrajectoryPlannerROS/global_plan", 1000);
ros::Rate loop_rate(1);
nav_msgs::Path gui_path;
geometry_msgs::PoseStamped pose;
std::vector<geometry_msgs::PoseStamped> plan;
for (int i=0; i<5; i++){
pose.pose.position.x = i;
pose.pose.position.y = -i;
pose.pose.position.z = 0.0;
pose.pose.orientation.x = 0.0;
pose.pose.orientation.y = 0.0;
pose.pose.orientation.z = 0.0;
pose.pose.orientation.w = 1.0;
plan.push_back(pose);
}
gui_path.poses.resize(plan.size());
if(!plan.empty()){
gui_path.header.frame_id = 'map';
gui_path.header.stamp = plan[0].header.stamp;
}
for(unsigned int i=0; i < plan.size(); i++){
gui_path.poses[i] = plan[i];
}
while (1){
path_pub.publish(gui_path);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
I can rostopic echo move_base/TrajectoryPlannerROS/global_plan
and get path like this:
header:
seq: 10
stamp:
secs: 0
nsecs: 0
frame_id: ''
poses:
-
header:
seq: 0
stamp:
secs: 0
nsecs: 0
frame_id: ''
pose:
position:
x: 0.0
y: 0.0
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
-
header:
seq: 0
stamp:
secs: 0
nsecs: 0
frame_id: ''
pose:
position:
x: 1.0
y: -1.0
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
But the robot in Rviz is not moving. Is my method wrong? Which topic should I use?
is your path appearing in Rviz? I am trying to do something similar