Homework 2

1. Add the following operations to Vector2D (referring to Operator Overloading Rules)
   • operator+= and operator+ (vector addition)
   • operator-= and operator- (vector subtraction)
   • operator*= and operator* (multiplication by a scalar)
     • The scalar can be to either the right or the left of the vector
   • double dot(Vector2D, Vector2D) (The dot product of two vectors)
   • double magnitude(Vector2D) (computer the magnitude of the vector)
   • double angle(Vector2D,Vector2D) (compute the angle between two vectors)
2. Write unit tests for these functions to make sure that they work as expected.

1. Add a function Transform2D integrate_twist(Twist2D) to turtlelib.
   • It should compute the transformation corresponding to a rigid body following a constant twist (in its original body frame) for one time-unit
   • The ability to integrate for exactly one time-unit allows integrating for arbitrary lengths of time by scaling the twist.
2. Include 3 unit tests for this function:
   1. One test should test pure translation
   2. One test should test pure rotation
   3. One test should test simultaneous translation and rotation

1. Create a DiffDrive class in turtlelib, in diff_drive.hpp and diff_drive.cpp.
2. This class (and any associated non-member functions) models the kinematics of a differential drive robot with a given wheel track and wheel radius.
   • It handles all the mathematical calculations for the robot kinematics without being tied to a specific robot or control scheme.
3. The class (and any associated non-member functions) should, at a minimum:
   1. Track the position of a robot's wheels \((\phi_r,\phi_l)\) and its configuration \(q = (x, y, \theta)\).
   2. (Forward Kinematics) Given new wheel positions \((\phi_r', \phi_l')\), update the configuration assuming that to get to the new position from the previous position:
      • Both wheels started rotating simultaneously
      • Both wheels stopped rotating simultaneously
      • The wheels rotated at constant velocity
      • The wheels achieved their velocity instantaneously
      • The wheels did not slip
      • The wheels rotated the shortest distance possible to get to their final configuration from the starting configuration
   3. (Inverse Kinematics) Compute the wheel velocities required to make the robot move at a given body twist.
      • If the twist cannot be accomplished without the wheels slipping throw an std::logic_error() exception.
   4. You may add other functionality to this class as needed/appropriate. These needs may arise in other tasks.
4. The DiffDrive class should use functions/classes declared in geometry2d.hpp and se2d.hpp as appropriate.
5. The DiffDrive class and associated functions should not use any ROS publishers, services, clients, subscribers, parameters, or other ROS packages.

1. Write unit tests for diff_drive for the forward and inverse kinematics. Consider at least the following scenarios:
   • The robot drives forward (both forward and inverse)
   • The robot executes a pure rotation (both forward and inverse)
   • The robot follows the arc of a circle (both forward and inverse)
   • An impossible-to-follow twist is provided.

1. Make sure everything in diff_drive.hpp has doxygen-style comments explaining how to use everything.
2. Be sure to mention diff_drive in the README.md
3. You will likely need to perform some mathematical calculations (involving twists, transforms, and adjoints) to accomplish this task:
   • Record your work in doc/Kinematics.pdf. It can be done in latex, or scanned hand-written notes, etc.)
   • Refer to your notes in your code, using comments to explain the origin of the mathematical expressions you use (labeling equations with numbers helps).

Write a node called turtle_control that enables control of the turtlebot via geometry_msgs/msg/Twist messages on the cmd_vel topic.

1. The node relies on parameters defined in nuturtle_description diff_params.yaml, as appropriate
   • If any of the needed parameters are not defined, it is an error and the node should print an error message and terminate.
   • To print messages that are logged by ROS, use ROS_ERROR_STREAM, ROS_INFO_STREAM, ROS_DEBUG_STREAM, etc. (See Logging).
2. It also relies on messages defined in nuturtlebot_msgs (https://github.com/ME495-Navigation/nuturtlebot_msgs.git).
   • Clone this repository into your source space ws/src, and checkout the humble branch
   • This code resides in the ws/src/nuturtlebot_msgs directory and contains message definitions for low-level turtlebot control
   • Use vcstool (install via apt as python3-vcstool) to export a .repos file from ws/src.
   • vcs export > turtle.repos. Save the turtle.repos in the base directory of your git repository.
   • Other people can then download the turtle.repos and use it to duplicate the contents of your ws/src directory.
3. The node subscribes to cmd_vel (geometry_msgs/msg/Twist) and publishes wheel_cmd (nuturtlebot_msgs/msg/WheelCommands) that will make the turtlebot3 follow the specified twist
4. The node subscribes to sensor_data (nuturtlebot_msgs/msg/SensorData) and publishes joint_states (sensor_msgs/msg/JointState) to provide the angle (in radians) and velocity (in rad/sec)
5. Use the DiffDrive class from turtlelib to implement this node

In this task, you will write a node called odometry that publishes odometry messages and the odometry transform.

1. The node reads the following parameters
   • body_id: The name of the body frame of the robot (e.g. "base_footprint"). If not specified, print an error and exit.
   • odom_id: The name of the odometry frame. Defaults to odom if not specified.
   • wheel_left: The name of the left wheel joint. If not specified, print an error and exit.
   • wheel_right: The name of the right wheel joint. If not specified, print an error and exit.
2. Every time the node receives a JointState message it should update its internal odometry state.
3. The node publishes a nav_msgs/msg/Odometry message on the odom topic:
   • Be sure to examine the message definition to understand the meaning of its fields
   • You do not need to fill out any covariance information (all covariance can be set to zero)
   • The pose should be specified in the odom_id frame
   • The velocity will be specified as the body velocity of the robot in the body_id frame
4. Broadcast the transform between odom_id and the body_id on /tf using a tf2 broadcaster
5. The odometry node should offer a service called initial_pose.
   • The service request provides the configuration of the robot.
   • The location of the odometry is reset so that the robot thinks it is at the requested configuration

1. Write a node called circle that publishes cmd_vel commands to cause the robot to drive in a circle of a specified radius at a specified speed.
2. The node offers a control service that takes the following parameters in the request:
   • velocity: The angular velocity, positive for counter-clockwise, negative is clockwise
   • radius: the radius of the arc
3. The node offers a reverse service (std_srvs/Empty) that reverses the direction of the robot along the arc.
4. The node offers a stop service (std_srvs/Empty) that stops the robot
5. When not stopped, The cmd_vel commands should be published at a fixed rate, determined by the frequency parameter, which defaults to 100Hz.
   • When stopped, Publish a single cmd_vel command of zero, then stop publishing

In this task, we will test the ROS API of the turtle_control and odometry nodes.

1. For this task, we will use the catch_ros2 package, which enables C++ integration testing using the catch framework
2. Write a test node called turtle_control_test that tests turtle_control ROS API
   • Write a test that verifies that cmd_vel commands with pure translation result in the appropriate wheel_cmd being published.
   • Write a test that verifies that cmd_vel commands with pure rotation in the appropriate wheel_cmd
   • Write a test that verifies that encoder data on sensors is converted to joint_states properly
   • Write a test launchfile file called test/turtle_control_test.launch.xml that runs turtle_control_test and turtle_control
3. Write a test node called turtle_odom_test_node to test the odemetry ROS API
   • Write one test case for the initial_pose service
   • Write one test case that uses a tf2_listener to verify that a transform from odom to a base_footprint is being published
     • The transform should be the identity transformation, the joint states will not change throughout the test
   • Write a test launch file called test/turtle_odom_test.launch.xml that runs a joint_state_publisher as well as the turtle_odom_test_node
   • Because the odometry node was implemented in terms of DiffDrive we will not test the the actual odometry calculations here
4. Write a test launch file called test/turtle_circle_test.launch.xml that verifies the frequency that the circle publishes cmd_vel commands.

Write a launchfile called start_robot.launch.xml that allows the user to cmd_vel commands to the turtlebot (real or simulated, remote or local), receive odometry, and visualize everything in rviz. The details are specified below. It is a good strategy to write pieces of this in parallel with your development of the nodes it needs to launch (some of which are done in later tasks). For example, work on the simulator-specific tasks first, and add real robot functionality later.

1. Write a launchfile that can be called like ros2 launch nuturtle_control start_robot.launch.xml cmd_src:=X robot:=Y use_rviz:=Z
   • The launchfile does different actios depending on its arguments
   • The argument cmd_src can be
     • circle to start the circle node
     • teleop to start the turtlebot3_teleop teleop_twist_keyboard
     • none start nothing, another node not started by this launchfile will publish the cmd_vel commands
   • The argument robot can be
     • nusim to start nusim simulator, the odometry node, and the turtle_control node. This is the default. (See Task C)
     • localhost To run the nodes directly from the turtlebot3 (See Task F for additional nodes that need to run)
       • With this option, the odometry and turtle_control nodes should run on the turtlebot, as well as the numsr_turtlebot node.
     • none: Don't launch any additional nodes
   • The argument use_rviz, if true, launches rviz with a configuration that enables seeing the robot model, tf frames, and the odometry.
     • use_rviz must be false when used with the robot:=localhost option
2. The launchfile should use a tf2_ros static_transform_publisher to publish an identity transform between the nusim/world and the odom frame
3. The odometry node should publish a transform between odom and blue/base_footprint.
4. rviz should always display the blue turtle
5. rviz should also display the red turtle, if robot == nusim

1. This task is optional
2. Unlike ROS 1, ROS 2 does not yet have a direct ability to launch nodes on remote machines using launchfiles
3. Use launch_remote to write a launchfile called remote_start.launch.xml.
   • It takes an argument host to specify the remote machine
   • It then launches all nodes needed on both your computer and on the remote machine
4. Feel free to submit pull requests to https://github.com/NU-MSR/launch_remote/tree/main
   • You may submit pull requests relating to documentation or functionality.
   • The best way of doing remote nodes in ROS 2 is unclear

The nusim node already loads and displays a robot in rviz. The goal of this task is to allow that robot to move, according to simulated kinematics, but through the same publisher/subscriber interface that an actual turtlebot uses.

1. The nusim node subscribes to red/wheel_cmd (nuturtlebot_msgs/msg/WheelCommands) to receive motion commands for the turtlebot
2. Each received wheel_cmd command sets the wheel velocities of the robot, until the next wheel_cmd command is received.
   • The wheel_cmd messages are integer values between -265 and 265 and are proportional to the maximum rotational velocity of the motor (see Specifications and A.8).
3. On each simulation run, the nusim should (in addition to what it already does)
   1. Update the wheel positions and publish them on red/sensor_data as a nuturtlebot_msgs/SensorData message.
      • For the time-being, you can ignore all the fields in this message other than the wheel positions and time stamp.
   2. Use the forward kinematics from the DiffDrive class to update the position of the actual (red) robot and publish the appropriate frame (assume no slipping)
4. After this functionality is implemented in the simulator, you should be able to drive the robot around using the teleop twist keyboard.
   • The Blue and Red robots will be overlayed, because we have not yet added noise or slipping to our simulation model and therefore the odometry is perfect.

1. Follow the Turtlebot3 Setup instructions to get your computer setup to use one of the turtlebots.
2. Add export ROS_DOMAIN_ID <YourID> to the ~/.bashrc on your computer.
   • The <YourID> is a number from 1 to 64 that is your unique id for the class
3. Running code on the turtlebot: (for example on leonardo)
   • ssh -oSendEnv=ROS_DOMAIN_ID msr@leonardo and source the install space.
   • You can now run launchfiles on the turtlebot as usual.
   • The node that starts the low-level turtlebot code is run with ros2 run numsr_turtlebot numsr_turtlebot
4. You should also be able to ros2 topic list from your laptop to see topics
   • Try echoing the sensor data
   • Not all fields in the sensor data are currently filled in. The ones you need are basically the encoder values.

1. Compiling code on the turtlebot itself is rather slow, so we will use cross-compilation
   • Cross-compilation is when you compile code on one platform that runs on another
2. For most of our tasks, we will use a docker container that has all the necessary tools and libraries for compiling aarch64 binaries for the turtlebot on the host system.
   • This will let us compile on our own computers, which can greatly speed up the compilation process
3. Follow the Cross compilation instructions to setup docker and the cross-compiling system.
   • Cross-compile your code on your machine
   • Use rsync to place the generated binaries on the turtlebot3
   • Use ssh to run the required launchfiles on the turtlebot3

1. Take a video and screencast of the following experiment and embed it in the github README.md
   • Credit will only be given for videos that play properly when embedded in the github README.md
   • Do not place the videos in your git repository: this will make the repository too large.
2. Drive the robot in a circle, clockwise and counter clockwise several times, stopping when the turtlebot is its initial configuration.
   • When you get close to stopping, you can use the teleop_keyboard to try to get the robot as close to its initial configuration as you can.
   • Record the final location of the turtlebot according to odometry in the README.md.
   • The distance from zero is a measure of the error in the odometry