Autonomy Starter Project Perception
One can think of an entire ROS project as a collection of nodes that talk to each other via named topics. Without any extra info, the data flowing between the nodes are just bytes. Messages identify how this data is structured (go ahead and read the linked Wiki page). We want to make a message that tells Navigation where the tag is. Here is one possible solution:
int32 tag_id
float32 x_tag_center_pixel
float32 y_tag_center_pixel
float32 closeness_metric
Take a look at this a file called StarterProjectTag.msg under the msg folder in the starter_project directory. There should be no other messages in there yet.
You may be asking now, how do I use this in C++? I just made some text file? CMake will automatically generate the C++ code for this message!
In AutonomyStarterProject.cmake take a look at:
file(GLOB_RECURSE STARTER_PROJ_MESSAGE_PATHS RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} CONFIGURE_DEPENDS
${CMAKE_CURRENT_LIST_DIR}/msg/*.msg
)which will tell CMake to generate all of the message files present in the msg directory.
Now run ./build.sh in terminal to build the new message file.
Direct your attention to the Perception::findTagsInImage function. Our first task will be to extract the ArUco tags from the image parameter and place them into the tags output vector.
You will want to use the cv::aruco::detectMarkers function for this. Read the hint to understand what parameters you need to pass.
Make sure to also fill in Perception::getClosenessMetricFromTagCorners and Perception::getCenterFromTagCorners. You should use these in the Perception::findTagsInImage.
Implementing these two functions will require some thought and we will not provide a way to do it. Discuss with your partners or others about how to solve both. x_tag_center_pixel and y_tag_center_pixel can be thought of as the center of the four corners of the tags, which you have access to via std::vector<cv::Point2f>. Note the types carefully! It is worth reading them in perception.hpp. For closeness metric, you only need an approximation. It will be used to drive towards the tag and stop within a distance. Consider how you as a human would estimate how far something is from you using your eyes. How would you approximate how far the vehicles in the image below are from you? How do you know they are different distances away? Be creative!
Next you will want to select the tag from this vector that is closest to the center of the camera. In other words the distance to (Perception::selectTag function.
Now that we have our desired tag, it is time to publish it to the proper topic. Implement Perception::publishTag.
To test your tag detection algorithm run ros2 launch mrover starter_project.launch.py to open the simulator. Then run ros2 topic echo /tag to monitor the output of perception. Make sure your node (the code you wrote) is not crashing in the log output!
First comment out launching our node in the starter_project.launch.py file. You will instead be launching it from VSCode.
Now run ros2 launch mrover starter_project.launch.py in a terminal.
Then hit Ctrl-Shift-P and run Cmake: Debug. Select "Unspecified" if it asks for a kit. Select the starter_project_perception target to run (these settings are also on the bottom bar).
Make sure to set breakpoints in the source code files! They are almost always better than print statements.
Consider the following image. Let's say it is 400x400 pixels. We can define a coordinate system that starts at the top left corner, consider that (0,0) with positive values of x extending right and positive values of y extending downward. The center of the tag would be about (300,100) in this space. Here is a diagram to aid understanding:
