Objectives

In this assignment, you will perform coordinate transforms to determine the location of an object seen by a sensor relative to the robot and to the world coordinate frames.

This assignment is split into two parts:

1. Setup the necessary transform and calculate the position of four points by hand. You may use tools such as a calculator, Matlab, Python, or Wolfram Alpha to do matrix multiplication but you must show your work as described below.
2. Create a ROS node (along with a launch file to start it) that takes in a duckietown_msgs/Vector2D message on a topic called /sensor_coord and:
   1. Publishes the transformed point in robot-relative coordinates to a new topic called exactly /robot_coord as a Vector2D message.
   2. Publishes the transformed point in world coordinates to a second new topic called exactly /world_coord as a Vector2D message.

Verify that your node gives the same answer as your calculations in Part 1.

Problem Description

Your robot is in position (3, 2) facing 45 degrees left of the y-axis (135 degrees relative to x-axis) in world coordinates. It has a sensor pointed exactly behind the robot (180 degrees from forward/positive x-axis on the robot) and placed 1 m behind the center of the robot.

The sensor returns obstacles in the following locations (in the sensor coordinate frame):

1. (18,8)
2. (1,-9)
3. (-11,12)
4. (-20,-18)

Instructions

1. Draw the robot and sensor in the world coordinate frame (does not need to be to scale).
2. Find the transform from the sensor to the robot coordinate frame.
3. Find the transform from the robot to the world coordinate frame.
4. Find the transform from the sensor to the world coordinate frame.
5. Calculate the position of each of the four points above relative to the robot coordinate frame.
6. Calculate the position of each of the four points above relative to the world coordinate frame.
7. Create a ROS node that performs these two transforms. As stated above, the node must subscribe to the topic /sensor_coord that has the Vector2D message type and publish as a Vector2D message:
   1. The position relative to the robot coordinate frame to /robot_coord
   2. The position relative to the world coordinate frame to /world_coord
8. Modify the homework 5 launcher script to start your code. You do NOT need to start anything except your node for this assignment.

Submission Instructions

Submit a PDF to Blackboard with:

1. The answers (along with any work) to questions 1-6 above.
2. Your name, repo URL, and the tag for this assignment
3. The roslaunch command that starts your code for this assignment.

Rubric

24 points: Answers to instruction questions 1-6 (4 points each)
12 points: ROS node/launch file to perform the transform for any Vector2D input
4 points: Submission details as defined above