Autonomy Starter Project Perception - umrover/mrover-ros Wiki
Creating a Custom Tag Message
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 tagId float32 xTagCenterPixel float32 yTagCenterPixel float32 closenessMetric
Make this a new 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? The answer is that you have to now add your file to the top of the
CMakeList.txt file present in the
starter_project directory. CMake will automatically generate the C++ code for this message!
add_message_files( FILES StarterProjectTag.msg <---- add your message here )
catkin build in terminal or hit Ctrl-Shift-P in VSCode and run
CMake: Build to generate the C++ code.
Detecting ArUco tags
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::getCenterFromTagCorners. You should use these in the
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.
yTagCenterPixel 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. Be creative!
Consider reading the contour features page in OpenCV to gather some ideas.
Selecting the Center-most Tag
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 (image width / 2, image height / 2) is smallest. Go ahead and fill in the
Publishing the Tag
Now that we have our desired tag, it is time to publish it to the proper topic. Implement
Testing your Work
First you will want to edit the world to place the tag right in front of the rover. Open
starter_project.world (Ctrl-P to search in VSCode) and find the following snippet:
... <model name='waypoint_post'> <pose>5.5 2.0 0.5 0 0 4.71239</pose> ...
The pose encodes the position and rotation of the tag in 3D space (X position Y position Z position X rotation Y rotation Z rotation). Change the position to
1.0 0.0 0.5 so it is right in front of the rover.
roslaunch mrover starter_project.launch to open the simulator. Then run
rostopic 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 file. You will instead be launching it from VSCode. Copy the
launch.json file from here: https://vector-of-bool.github.io/docs/vscode-cmake-tools/debugging.html. Run
roslaunch mrover starter_project.launch in one terminal. Then run the launch configuration we created from VSCode, it should start debugging. Make sure to set breakpoints, they are almost always better than print statements.
What is Camera Space?
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: