ros/src/visualize/visualize.py
def loop(self):
        """
        publish marker of final waypoints from car's perspective
        """
        rate = rospy.Rate(50)
        while not rospy.is_shutdown():
            if self.current_pose and self.final_waypoints:
                self.marker_object.points = []
                for i, wp in enumerate(self.final_waypoints.waypoints):
                    roll, pitch, yaw = euler_from_quaternion([self.current_pose.pose.orientation.x,
                                                              self.current_pose.pose.orientation.y,
                                                              self.current_pose.pose.orientation.z,
                                                              self.current_pose.pose.orientation.w])
                    temp_point = Point()
                    #get relative waypoint position(x, y) from current position
                    p_x = wp.pose.pose.position.x - self.current_pose.pose.position.x
                    p_y = wp.pose.pose.position.y - self.current_pose.pose.position.y
                    #perspective transform (map coord -> car coord)
                    #(+) direction of x axis->yaw=0, Clock-wise->yaw (+), Counter Clock-wise->yaw (-)
                    cos_yaw = math.cos(-yaw)
                    sin_yaw = math.sin(-yaw)
                    temp_point.x = p_x * cos_yaw - p_y * sin_yaw
                    temp_point.y = p_x * sin_yaw + p_y * cos_yaw
                    temp_point.z = 0.0
                    self.marker_object.points.append(temp_point)

                self.marker_publisher.publish(self.marker_object)
            rate.sleep()

        publish marker of final waypoints from car's perspective
        
publish marker of final waypoints from car's perspective
publish marker of final waypoints from car ' s perspective
def loop(self):
        rate = rospy.Rate(50)
        while not rospy.is_shutdown():
            if self.current_pose and self.final_waypoints:
                self.marker_object.points = []
                for i, wp in enumerate(self.final_waypoints.waypoints):
                    roll, pitch, yaw = euler_from_quaternion([self.current_pose.pose.orientation.x,
                                                              self.current_pose.pose.orientation.y,
                                                              self.current_pose.pose.orientation.z,
                                                              self.current_pose.pose.orientation.w])
                    temp_point = Point()
                    p_x = wp.pose.pose.position.x - self.current_pose.pose.position.x
                    p_y = wp.pose.pose.position.y - self.current_pose.pose.position.y
                    cos_yaw = math.cos(-yaw)
                    sin_yaw = math.sin(-yaw)
                    temp_point.x = p_x * cos_yaw - p_y * sin_yaw
                    temp_point.y = p_x * sin_yaw + p_y * cos_yaw
                    temp_point.z = 0.0
                    self.marker_object.points.append(temp_point)
                self.marker_publisher.publish(self.marker_object)
            rate.sleep()
def loop ( self ) : rate = rospy . Rate ( 50 ) while not rospy . is_shutdown ( ) : if self . current_pose and self . final_waypoints : self . marker_object . points = [ ] for i , wp in enumerate ( self . final_waypoints . waypoints ) : roll , pitch , yaw = euler_from_quaternion ( [ self . current_pose . pose . orientation . x , self . current_pose . pose . orientation . y , self . current_pose . pose . orientation . z , self . current_pose . pose . orientation . w ] ) temp_point = Point ( ) p_x = wp . pose . pose . position . x - self . current_pose . pose . position . x p_y = wp . pose . pose . position . y - self . current_pose . pose . position . y cos_yaw = math . cos ( - yaw ) sin_yaw = math . sin ( - yaw ) temp_point . x = p_x * cos_yaw - p_y * sin_yaw temp_point . y = p_x * sin_yaw + p_y * cos_yaw temp_point . z = 0.0 self . marker_object . points . append ( temp_point ) self . marker_publisher . publish ( self . marker_object ) rate . sleep ( )
publish marker of final waypoints from car's perspective
publish marker of final waypoints from car ' s perspective
"""
        publish marker of final waypoints from car's perspective
        """ #get relative waypoint position(x, y) from current position #perspective transform (map coord -> car coord) #(+) direction of x axis->yaw=0, Clock-wise->yaw (+), Counter Clock-wise->yaw (-)
self