For this nav_msgs/Path
message you can use code similar to this one:
def publish_path(self):
msg = Path()
msg.header.frame_id = "/map"
msg.header.stamp = rospy.Time.now()
for wp in your_global_plan_world_coordinates_array:
pose = PoseStamped()
pose.pose.position.x = wp[0].location.x
pose.pose.position.y = -wp[0].location.y
pose.pose.position.z = wp[0].location.z
quaternion = tf.transformations.quaternion_from_euler(
0, 0, -math.radians(wp[0].rotation.yaw))
pose.pose.orientation.x = quaternion[0]
pose.pose.orientation.y = quaternion[1]
pose.pose.orientation.z = quaternion[2]
pose.pose.orientation.w = quaternion[3]
msg.poses.append(pose)
rospy.loginfo("Publishing Path.")
self.waypoint_publisher.publish(msg)
In general, you need to have your messages PoseWithCovarianceStamped
in an array, which you then append to the message in a for
loop iterating through elements in your publisher