# Tutorial 4 ## Waypoint Publishing and Following --- ### Waypoint Publisher To begin, we need first need to set up our file structure. !!! Note that we are working with C++ to be able to compile our own messages in the future if desired. This being said, the following can be completed in Python if desired. !!! Set up a new C++ package with a node. Call them both `waypoint_publisher`. ```bash ros2 pkg create --build-type ament_cmake --node-name ``` Inside your project root, create a folder called `data`. This is where our waypoints will be stored. You should populate it with waypoints files (`.csv`) from [f1tenth_racetracks](https://github.com/f1tenth/f1tenth_racetracks). We will be creating our waypoint lines later. We will be using the centerline data for this tutorial. Check `src/f1tenth_gym_ros/config/sim.yaml` to see what map you currently have active. Once completed, your file structure should look something like this: ![](/static/uploads/tutorial-4/dir-structure.png) You do not need all of these tracks, just the active one's centerline (see above). Code source: https://gitlab.msu.edu/canvas/soar/public/f1tenth/-/blob/main/src/waypoint_publisher/src/waypoint_publisher.cpp We are going to work through the code. Note that it is C++ but the ROS convention is very similar to Python. ```cpp # waypoint_publisher.cpp class WaypointPublisher : public rclcpp::Node { public: WaypointPublisher() : Node("waypoint_publisher") { publisher_ = this->create_publisher("waypoints", 10); timer_ = this->create_wall_timer(1s, std::bind(&WaypointPublisher::timer_callback, this)); std::string file_name = "Spa_slammed_mincurv.csv"; std::string file_loc = get_file_loc(file_name); RCLCPP_INFO(this->get_logger(), "File location: %s", file_loc.c_str()); path = get_waypoints(file_loc, file_name); RCLCPP_INFO(this->get_logger(), "Loaded %d waypoints", (int)path.poses.size()); this->timer_callback(); } nav_msgs::msg::Path path; ``` Shown above is the public portion of our node. Lines 1-5 handle defining our node class. We are declaring it as a child of "Node" (a ROS class) and passing our node's name ("waypoint_publisher"). Pretty simple stuff, most C++ ROS nodes will begin in this fashion. On line 6 we declare a publisher which communicates on the "/waypoints" topic with "Path" messages (which is just an array of waypoints). Line 7 creates a 1 second timer for a function called `timer_callback` which we will define soon. This is conventional code that is used in ROS C++ nodes. Line 9 declares a string that describes the filename that contains the set of waypoints we want to publish. Line 17 calls our `timer_callback` function so that we don't have to wait 1 second when our node launches to get our waypoints published. `timer_callback` is simply: ```cpp # void timer_callback() { publisher_->publish(path); RCLCPP_INFO(this->get_logger(), "Publishing %d waypoints", (int)path.poses.size()); } ``` Line 11 contains a simple function which gets the absolute path of our waypoint file after it is compiled. It is defined as: ```cpp # std::string get_file_loc(std::string file_name) { return std::string((std::string)ament_index_cpp::get_package_share_directory("waypoint_publisher") + "/data/" + file_name); } ``` Last but not least, line 14 calls an essential function: `get_waypoints`. This function reads our `.csv` waypoint file and converts it to a `Path` object. ```cpp # nav_msgs::msg::Path get_waypoints(std::string filepath, std::string filename) { nav_msgs::msg::Path new_path; path.header.frame_id = "map"; path.header.stamp = this->now(); FILE *fp; fp = fopen(filepath.c_str(), "r"); if (fp == NULL) { RCLCPP_ERROR(this->get_logger(), "Failed to open file: %s", filepath.c_str()); return new_path; } char buf[256]; while (fgets(buf, sizeof(buf), fp) != NULL) { double x, y, v; if(filename.find("centerline") != std::string::npos) { sscanf(buf, "%lf, %lf", &x, &y); v = 0; } else if(filename.find("mincurv") != std::string::npos) { sscanf(buf, "%lf,%lf,%lf", &x, &y, &v); } else { // raceline sscanf(buf, "%*s %lf %lf", &x, &y); v = 0; } geometry_msgs::msg::PoseStamped pose; pose.header = path.header; pose.pose.position.x = x; pose.pose.position.y = y; pose.pose.position.z = v; // orientation if (new_path.poses.size() > 0) { std::vector orientation = get_orientation(new_path.poses[new_path.poses.size()-1], pose); pose.pose.orientation.x = orientation[0]; pose.pose.orientation.y = orientation[1]; pose.pose.orientation.z = orientation[2]; pose.pose.orientation.w = orientation[3]; } else { pose.pose.orientation.x = 0.0; pose.pose.orientation.y = 0.0; pose.pose.orientation.z = 0.0; pose.pose.orientation.w = 1.0; } new_path.poses.push_back(pose); } return new_path; } ``` We begin by creating our `Path` object then opening our file. We then read through each line and extract variables x, y, and v (if available). Then we simply assign the coordinates to a `PoseStamped`, get the orientation (based on the direction from the last point) and push it to our Path's poses vector. Finally, at the end, we return the `Path`. The `get_orientation` function is simple and can be found in the source code. Lastly, our `cpp` file needs a main function. Once again, this is boilerplate code and can be copied for future ROS C++ nodes. ```cpp # int main(int argc, char ** argv) { rclcpp::init(argc, argv); rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; } ``` The necessary imports for this node are: ```cpp # #include #include #include #include #include #include #include using namespace std::chrono_literals; ``` The using chrono literals namespace is so we can use "1s" for our timer. We then update `package.xml` to look like so: ```xml # package.xml waypoint_publisher 0.0.0 TODO: Package description alexdalat TODO: License declaration ament_cmake rclcpp std_msgs nav_msgs ament_lint_auto ament_lint_common ament_cmake ``` Then, make sure the package compiles the `data` directory so our node can read them at runtime. Simply append: ```cmake # # Include data/ folder install(DIRECTORY data DESTINATION share/${PROJECT_NAME}) ) ``` Additionally, we need to link any libraries we are using such as std_msgs and nav_msgs. Your `CMakeLists.txt` should look like the following: ```cmake # CMakeLists.txt cmake_minimum_required(VERSION 3.8) project(waypoint_publisher) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(nav_msgs REQUIRED) # Create executable add_executable(waypoint_publisher src/waypoint_publisher.cpp) # Link dependencies ament_target_dependencies(waypoint_publisher rclcpp std_msgs nav_msgs) # Include data/ folder install(DIRECTORY data DESTINATION share/${PROJECT_NAME}) # Include header files target_include_directories(waypoint_publisher PUBLIC $ $) # Require C99 and C++17 target_compile_features(waypoint_publisher PUBLIC c_std_99 cxx_std_17) # Install executable to lib/ for ros2 run install(TARGETS waypoint_publisher DESTINATION lib/${PROJECT_NAME}) ament_package() ``` When you compile and run the node, the following output should appear and continue logging every second. ![](/static/uploads/tutorial-4/publisher-output.png) That's it for our waypoint publisher package! You can visualize it by toggling the `waypoints` topic in Foxglove Studio. --- ### Waypoint Follow Set up a new Python (yay!) package with a node. Call them both `waypoint_follow`. ```bash ros2 pkg create --build-type ament_python --node-name ``` Programming this node will be more interpretive but we will provide a template and the solution can be found on the GitLab. By adding a raw messages panel to foxglove we can observe our `/waypoints` messages format for the first couple of waypoints: ![](/static/uploads/tutorial-4/waypoints-msg.png) We can create a subscriber and store the points into our waypoint handler: ```py waypoint_follow.py self.got_waypoints = False # subscribe to waypoint publisher self.create_subscription( Path, "/waypoints", self.waypoint_callback, 10 ) ... def waypoint_callback(self, msg): if(not self.got_waypoints): self.get_logger().info("Received waypoints!") self.got_waypoints = True # continue updating in case path changes for obstacles self.navigator.waypoint_handler.update_waypoints(msg) ``` Then iterate through our waypoints like so: ```py WaypointHandler.py point_in_front = (car_pos[0] + math.cos(car_yaw) * dist, car_pos[1] + math.sin(car_yaw) * dist) for waypoint in self.waypoints.poses: distance = math.sqrt((waypoint.pose.position.x - point_in_front[0])**2 + (waypoint.pose.position.y - point_in_front[1])**2) if distance < closest_distance: closest_distance = distance closest_waypoint = waypoint ``` The rest of the waypoint handler can be found below. The meat of the code comes by creating a ROS timer which will act out our car's commands. Create a timer for your drive function that is executed 100 times per second. Inside the drive method, check that you have waypoints, otherwise you should skip (return). Lastly, you must create a file named WaypointNavigator.py that defines a class called WaypointNavigator. The base should look as such: ```py class WaypointNavigator: def __init__(self): self.car_length = 0.3302 self.current_steering = 0.0 self.current_speed = 0.0 self.waypoint_handler = WaypointHandler() def get_drive(self, car_odom): pass ``` Using https://thomasfermi.github.io/Algorithms-for-Automated-Driving/Control/PurePursuit.html and methods in WaypointHandler (specifically, `get_next_waypoint`), implement steering controls so that the car successfully steers towards the given waypoint. The essentials to pure pursuit are as follows. The vehicle's leftmost wheels are portrayed in blue. TC = target point, ICR = instantaneous center of rotation. ![](/static/uploads/tutorial-4/PurePursuit.png) Through trigonometry, a steering angle can be derived to direct the vehicle towards the point at distance l_d. Once complete, attempt to implement velocity regulation through lidar sensing or other techniques. ==- WaypointHandler.py ```c++ import math class WaypointHandler: def __init__(self): self.waypoints = None self.car_length = 0.3302 def get_next_waypoint(self, car_pos, car_yaw, dist = 0.6604): """ Returns next waypoint in front of the car """ if(self.waypoints is None): return None, 0.0, 0.0 # distance of point inf front of car based closest_waypoint = None closest_distance = float('inf') point_in_front = (car_pos[0] + math.cos(car_yaw) * dist, car_pos[1] + math.sin(car_yaw) * dist) for waypoint in self.waypoints.poses: distance = math.sqrt((waypoint.pose.position.x - point_in_front[0])**2 + (waypoint.pose.position.y - point_in_front[1])**2) if distance < closest_distance: closest_distance = distance closest_waypoint = waypoint if closest_waypoint is None: return None, 0.0, 0.0, 0.0 angle, distance = self.get_target(car_pos, closest_waypoint) waypoint_angle = euler_from_quaternion((closest_waypoint.pose.orientation.x, closest_waypoint.pose.orientation.y, closest_waypoint.pose.orientation.z, closest_waypoint.pose.orientation.w))[2] return closest_waypoint, angle, waypoint_angle, distance def angle_check(self,angle): while angle > math.pi: angle -= 2 * math.pi while angle < -math.pi: angle += 2 * math.pi return angle def get_relative_angle(self, car_yaw, angle): relative_angle = angle - car_yaw relative_angle = self.angle_check(relative_angle) return relative_angle def get_target(self, car_pos, next_waypoint): """ Returns angle from car position to waypoint position and distance between """ dx = next_waypoint.pose.position.x - car_pos[0] dy = next_waypoint.pose.position.y - car_pos[1] return math.atan2(dy, dx), math.sqrt(dx**2 + dy**2) def update_waypoints(self, waypoints): self.waypoints = waypoints ``` ==- ==- waypoint_follow.py ```py import rclpy from rclpy.node import Node from nav_msgs.msg import Odometry, Path from ackermann_msgs.msg import AckermannDriveStamped from .WaypointNavigator import WaypointNavigator class FollowWaypoint(Node): """ Minimal publisher class """ def __init__(self): super().__init__("waypoint_follow") self.get_logger().info("Waypoint Follow Node Started") # publish to a car's drive self.drive_topic = "/drive" self.publisher = self.create_publisher( AckermannDriveStamped, self.drive_topic, 1 ) self.get_logger().info("Publishing to %s" % self.drive_topic) # subscribe to a car's odom self.odom_topic = "/odom" self.subscriber = self.create_subscription( Odometry, self.odom_topic, self.odom_callback, 10 ) # subscribe to waypoint publisher self.waypoint_topic = "/waypoints" self.waypoint_subscriber = self.create_subscription( Path, self.waypoint_topic, self.waypoint_callback, 10 ) self.get_logger().info("Waiting for waypoints...") # how often we will send commands to the car timer_period = 0.01 self.timer = self.create_timer(timer_period, self.timer_callback) # waypoint navigator self.navigator = WaypointNavigator() self.car_odom = Odometry() self.got_odom = False self.got_waypoints = False def timer_callback(self): """ Drive timer callback """ if not self.got_odom or not self.got_waypoints: return timestamp = self.get_clock().now() speed, steering_angle = self.navigator.get_drive(self.car_odom) msg = AckermannDriveStamped() msg.header.stamp = timestamp.to_msg() msg.drive.speed = speed msg.drive.steering_angle = steering_angle self.publisher.publish(msg) self.got_odom = False # we want odom to be fresh every time we run def odom_callback(self, msg): self.car_odom = msg self.got_odom = True def waypoint_callback(self, msg): if(not self.got_waypoints): self.get_logger().info("Received waypoints for the first time!") self.got_waypoints = True # continue updating in case path changes for obstacles self.navigator.waypoint_handler.update_waypoints(msg) def main(args=None): """ Main """ rclpy.init(args=args) follow_waypoint = FollowWaypoint() rclpy.spin(follow_waypoint) follow_waypoint.destroy_node() rclpy.shutdown() if __name__ == "__main__": main() ``` ==-