ME495: Embedded Systems in Robotics

1. The turtle_control package is an ament_python package and should be located at ${ws}/src/homework1/turtle_control
   • This package has an exec_depend on rclpy and turtlesim
2. The turtle_interfaces package is an ament_cmake package and should be located at ${ws}/src/homework1/turtle_interfaces
3. Use ros2 pkg create to create the packages.
4. Edit package.xml to change the default but required fields to reflect your name, email, and a description of each package.
5. As you develop the package make sure to maintain consistency with package.xml and add any extra dependencies as they are needed.

1. Create a node called waypoint that (for now) logs a DEBUG message that says "Issuing Command!" at a fixed frequency using a timer.
   • This node will be expanded throughout this assignment and will form the base of the project.
   • The frequency should be specified as a parameter called "frequency" and should default to 90 Hz.
   • You will only see DEBUG messages when the node is run in a mode that enables the DEBUG logger level.
2. Make sure you can run the node with ros2 run

1. Using the std_srvs/Empty Service type, create a service called toggle and have the waypoint node offer it.
   • The node will now have two states: MOVING and STOPPED. It starts in the STOPPED state.
   • When toggle is called, the node switches states.
     • When switching from MOVING to STOPPED the node INFO logs "Stopping." one time.
     • While in the "MOVING" state, the node DEBUG logs "Issuing Command!" repeatedly
     • While moving the turtle should leave a visual trail behind to show where it has been
2. Your code should not attempt to stop the timer or create multiple timers: instead it should change what the timer is doing based on whether the state is MOVING or STOPPED.

1. Create a custom service of type turtle_interfaces/srv/Waypoints in the turtle_interfaces package that allows you to send an arbitrary number of waypoints to a node and responds with the total straight-line distance that the turtle will travel to complete one cycle of the waypoints (i.e., starting and ending at the same place).
   • The fields in this service are defined as follows:
   • The request has a waypoints field that is an array of geometry_msgs/msg/Point objects. The z field of the point is ignored.
   • The response has a distance field of type float64
   • Do not forget to update the package.xml and CMakeLists.txt as needed for the turtle_interfaces package.
2. Add a service called load of the turtle_interfaces/srv/Waypoints type to the waypoint node. This service shall:
   • Reset the turtlesim node to its initial state.
   • Draw an X at each waypoint.
   • Place the turtle at the first waypoint. The node should be STOPPED and turtle should not move.
   • Compute the straight-line distance from traversing one complete cycle of the waypoints and return that as a response.

1. Subscribe to the turtlesim topic that provides the position and orientation of the turtle.
2. This subscriber should store the received data for later use
3. The subscriber should neither publish any messages nor call any services.

1. When the toggle service puts the node into the MOVING state, the turtle should start following the most recently loaded waypoints.
   • If no waypoints are loaded log an ERROR message saying No waypoints loaded. Load them with the "load" service.
2. When MOVING, you should publish cmd_vel commands at the control loop frequency.
   • The cmd_vel velocities must have linear.y = 0 to emulate a differential-drive robot.
3. If the turtle is STOPPED (by another call to toggle) it should stop moving.
   • Resuming (by another call to toggle) should have the turtle continue along the waypoints.
4. A turtle reaches a waypoint if it is within some threshold distance of that the waypoint
   • This threshold should be set via a tolerance parameter to the node
5. The turtle's path should be visible when following the waypoints

1. Create a custom message type called ErrorMetric in the turtle_interfaces package that contains 3 values:
   • complete_loops: An integer counting the number of times the turtle has cycled through all the waypoints.
   • actual_distance: The actual distance traveled (this can be estimated as a linear interpolation between position feedback subscriber callbacks).
   • error: When a loop is completed, compute the difference between the straight-line distance of the path and the actual distance traveled.
2. Publish this message on the /loop_metrics topic every time the turtle completes a new cycle of the waypoints.
   • In this sense, the ErrorMetric is only updated once per full cycle.
   • The actual_distance and error are cumulative (i.e., if the turtle has completed 3 loops this is the actual distance traveled for the 3 loops and the error over that time).
3. All metrics should be reset whenever the /load service is called.
4. Your goal is to implement a control with the error metric after one loop as close to zero as possible.

Write an xml launchfile called waypoints.launch.xml (put it in the launch/ directory of the turtle_control package). The launchfile should:

1. Start the turtlesim_node and the waypoint node.
2. Load parameters from colors.yaml (stored in the config/ directory of the turtle_control package) to set the colors of the turtlesim_node.
   • The color you set should be visually distinct from the default value (e.g., it should not primarily be a shade of blue).
3. Specify the following parameters for waypoints
   1. frequency should be 100 Hz
   2. tolerance should be, at most, 0.05m

You should capture an example run, according to the following instructions:

1. The turtle traverses the following waypoints: (3.9, 5.4), (1.4, 1.6), (2.2, 9.4), (7.2, 6.1), (4.0, 2.6), (8.2, 1.5)
2. You stop and resume the turtle one time.
3. The turtle completes more than 1 but less than 2 cycles.

1. Capture a screen-cast and a ros2 bag capture of /turtle1/cmd_vel while performing this experiment.
   • Save the bag to homework1/turtle.bag and be sure to include it in the git repository
2. Manually reset the turtlesim and kill the waypoint node.
3. Use ros2 bag to play-back the bagged data to make the turtle move.
   • Capture a screen-cast of the turtle moving based on bag data.

In the base directory of your repository (e.g. homework1) create a README.md using the following template (be sure to update the ${} with the answers.)

# ME495 Embedded Systems Homework 1
Author: ${Your Name}
1. Use `ros2 launch ${pkg} ${launchfile}` to run the code
2. The `${ros2 service call /load <put arguments here that you used to make the video>}` service loads waypoints for the turtle to follow
3. The `${ros2 service call /stop <put arguments here that you used to make the video>}` starts and stops the turtle.
4. Here is a video of the turtle in action.

   ${embed video here, must show up inline in the README.md when rendered on github. Video file itself should be uploaded as a github issue and linked here, not in the repository}`

5. Here is a video when the bag is played.

   ${embed video here, must show up inline in the README.md when rendered on github. Video file itself should be uploaded as a github issue and linked here, not in the repository}`