parameters: (self)
docstring: Publish the ROS message containing the waypoints
docstring_summary: Publish the ROS message containing the waypoints
docstring_tokens: Publish the ROS message containing the waypoints
function: def publish_waypoints(self):
          """
          Publish the ROS message containing the waypoints
          """
          msg = Path()
          msg.header.frame_id = "map"
          msg.header.stamp = rospy.Time.now()
          if self.current_route is not None:
              for wp in self.current_route:
                  pose = PoseStamped()
                  pose.pose.position.x = wp[0].transform.location.x
                  pose.pose.position.y = -wp[0].transform.location.y
                  pose.pose.position.z = wp[0].transform.location.z
  
                  quaternion = tf.transformations.quaternion_from_euler(
                      0, 0, -math.radians(wp[0].transform.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)
  
          self.waypoint_publisher.publish(msg)
          rospy.loginfo("Published {} waypoints.".format(len(msg.poses)))
function_tokens: def publish_waypoints ( self ) : msg = Path ( ) msg . header . frame_id = "map" msg . header . stamp = rospy . Time . now ( ) if self . current_route is not None : for wp in self . current_route : pose = PoseStamped ( ) pose . pose . position . x = wp [ 0 ] . transform . location . x pose . pose . position . y = - wp [ 0 ] . transform . location . y pose . pose . position . z = wp [ 0 ] . transform . location . z quaternion = tf . transformations . quaternion_from_euler ( 0 , 0 , - math . radians ( wp [ 0 ] . transform . 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 ) self . waypoint_publisher . publish ( msg ) rospy . loginfo ( "Published {} waypoints." . format ( len ( msg . poses ) ) )