\documentclass[10pt]{article}
\usepackage{graphicx}
\usepackage[margin=1in]{geometry}


\begin{document}
\begin{center} \Large\sc
\end{center}
\section*{Visual Servoing}

In this laboratory exercise, you will control the motion of your robot based on visual feedback from your Zed camera.  This technique is called ``Visual Servoing" which typically classifies into two categories: Image Based (IBVS) and Position Based (PBVS). This lab has the following objectives:

\begin{enumerate}
\item Identify a bright orange cone with the zed camera using your own object detector
\item Implement IBVS to park your robot in front of a cone
\end{enumerate}
~
{\em Looking Ahead:} In lab 4B you will use OpenCV with ROS for more sophisticated image detection algorithms and implement PBVS. 
\section{Detect a Cone}
Objective: Write an image detector to detect an orange cone.
\subsection{Launching Zed Camera}
Launch the zed camera:
\begin{verbatim}
  roslaunch zed_wrapper zed_depth.launch
\end{verbatim}
Verify you can see the image from the zed camera. Using rqt\_image\_view and select the following topic from the drop down menu:
\begin{verbatim}
  "/camera/zed/rgb/image_rect_color"
\end{verbatim}
If you cannot view the image please refer to the steps for streaming data in Lab 2B. 
\subsection{Accessing Image Data}
Write a ros subscriber for zed camera topic: 
\begin{verbatim}
  "/camera/zed/rgb/image_rect_color"
\end{verbatim}
The zed camera publishes message of type Image from sensor\_msgs.  Learn about this message type by opening the file Image.msg located in 
  \begin{verbatim}
    roscd sensor_msgs/msg
  \end{verbatim}
Image.msg has the information about how to access the pixels of the image.\\
\subsection{Detect the cone}
Use your favorite ros language to write a subscriber for the zed camera messages. Refer to image.msg to access the pixels of the image and create a detector to determine the location of an orange cone in an image.\\


\section{Image Based Visual Servoing (IBVS)}
In this part of the lab you will park your robot in front of an orange cone.  The robot will have the cone in its field of vision and should drive directly to the cone and park in front of it.  You will implement this using image based visual servoing (IBVS). In IBVS, the control law is based on the error between current and desired features on the image plane, and {\em does not involve any estimate of the pose of the target.} \\
Since the zed camera is mounted symmetrically on the robot we want the cone to show up in the center of the image it captures. 

From Lab 1 we saw we can control the racecar using teleop.launch and a rostopic publisher. The commands are repeated here for your convenience:
\begin{verbatim}
  roslaunch racecar teleop.launch
  
  rostopic pub /vesc/ackermann_cmd_mux/input/navigation
  ackermann_msgs/AckermannDriveStamped 
  '{header: auto, drive: {steering_angle: .2, speed: 1.2} }'
\end{verbatim}

\subsection{Combine Object Detector and Motor Commands}
Use the object detector created in part 1.3 to determine which direction the robot should move to center itself in front of the cone.  Combine this with the teleop commands to drive the robot to the cone.  
\\
The robot should not run over the cone!  Combine some of the features from the previous lab to make sure it parks in front of the cone.
\end{document}

\begin{center} \Large\sc
6.141/16.405 Robotics: Science and Systems
\end{center}

\section*{Laboratory Exercise 4: Visual Servoing}

In this laboratory exercise, you will control the motion of your robot based on visual feedback from your Zed camera.  This technique is called ``Visual Servoing" which typically classifies into two categories: Image Based (IBVS) and Position Based (PBVS). 

\begin{enumerate}
\item Implement IBVS to park your robot in front of a cone\\{\em Expected Completion Date: 3/2}
\item Implement PBVS to avoid a collision with cone by moving to the left or right of the cone.\\{\em Expected Completion Date: 3/7}
\item Use PBVS to complete a pole bending course.  \\{\em Expected Completion Date: 3/9} 
\item {\bf Optional: Compete in a pole bending competition during lab! \\{\em Competition Date: 3/9} }
\end{enumerate}

\section{Detect a Cone}
There are many object detection algorithms!  You are able to use any algorithm or method your wish to implement.  We have prepared a directory that includes the correct CMakeList to include opencv2.4 code.  Additionally we have two .cpp files that show how to use SURF feature detectors or template matching and one python file with a template matching example.
Included in the python file is an example to use cvbridge to subscribe to the zed camera feed.


\section{Image Based Visual Servoing (IBVS)}
In this part of the lab you will park your robot in front of an orange cone.  The robot will have the cone in its field of vision and should drive directly to the cone and park in front of it.  You will implement this using image based visual servoing (IBVS). In IBVS, the control law is based on the error between current and desired features on the image plane, and {\em does not involve any estimate of the pose of the target.} \\
Since the zed camera is mounted symmetrically on the robot we want the cone to show up in the center of the image it captures. 

\section{Position Based Visual Servoing (PBVS)}
In this lab we want to estimate the relative distance of the cone to the robot. Use this information to drive the robot .2 meters to the right or left of the cone.

Once you have successfully completed avoiding a cone try avoiding two cones in a row.  If your segmentation is having difficulty with multiple cones in the image, try using the depth information to determine which cone is closest to your robot.


\section{Pole Bending with PBVS}
Next, program the robot to switch which direction it uses to circumvent the cone.  e.g.: move to the right of the cone, then to the left of the cone like a snake:
The swivel through multiple cones is the minimum spec required to complete this lab.  \\
The next objective is to program the robot to complete a pole bending course
(see figures)













\end{document}
