====== Robotino Object Seeking ======

**This tutorial is part of the [[robotino_operation|Robotino Operation]] series.**

The completed project can be downloaded here {{dylanw:robotino_control_2.zip}}

This is an extension of the [[robotino_vision_pickup|Robotino Vision Pickup]] tutorial.  It is recommended that you complete that tutorial before proceeding.

===== Videos =====

The completed project in action.

{{youtube>BI-5vv5YfBA?large}}
{{youtube>KmJhrMGxEa0?large}}\\ 

===== Structure of the program =====

The updated program is split into 3 files approach_grab.cpp, main.cpp  (in the src directory) and approach_grab.h (in the include directory).

You need to modify your CMakeList.txt file by adding the following lines

  rosbuild_add_executable(
  approach_grab_node
  src/approach_grab_node.cpp
  src/approach_grab.cpp
  )

**approach_grab.h**

This file contains the class prototype used.  Please download the completed project {{dylanw:robotino_control_2.zip}} and look through this file.  It contains ample documentation on the purpose of each function and variable.  However, there are a few Functions worth going over in more detail.

**spin()**

when this function is called the class will begin to operate under it's default operating mode.  You can think of this as your main program loop.

**Pose Handeling**

  struct pose{
    float array[6];
    bool gripperClosed;
  } BHAPose;
  
  void setBHAPose(pose myPose);   	// set the bhapose using a pose object 
  void setBHAPose(float x0,float x1,float x2,float x3,float x4,float x5, bool gripper); // set bha pose using primitives
  void sendPoseMsg();

In order to handle BHAPose messages more easily across funcitons a "pose" structure was created and declared as BHApose.   In order to set the pose you can use 1 of two variants of setBHAPose.  One function takes a pose object.  this is useful if you want to do operations on the pose structure before you send it.  Alternatively, you can simply set the pose using c primitives with the second version of the function.

SendPoseMsg() will simply send BHAPose as a message.  So you will want to use this if you set BHAPose manually or want to continuously send the last pose sent

===== Details of some of the functions =====

Some of the more simple functions in the class are omitted from this section because they are self-explainitory with the included comments.

<code c++ spin.cpp>
void approach_grab::spin(){
    
    ros::Rate loop_rate( 10 );
    
    while(ros::ok()){
        //setRobotinoVelocity(0,0,0);
        setBHAPose(0.591398, 0.522972, 0.532747, 0.796676, 0.821114, 0.904203,false);
      
        ROS_INFO("dist to target %i,%i",distToTarget[0],distToTarget[1]);
        float omega;
        
        if(distToTarget[1]<=0)
            omega=.5;
        else
            omega=-.5;
            
        //  wait for the claw to open and for the ball to roll away.
        ros::Duration(5).sleep();
        // reset the dist to target for a new loop iteration
        distToTarget[0]=0;
        distToTarget[1]=0;
        ROS_INFO("dist to target %i,%i",distToTarget[0],distToTarget[1]);
        ROS_INFO("Looking For Target Object");
      
        // if target is not found rotate to look for it.
        while(distToTarget[0]==0||distToTarget[1]==0){
            setRobotinoVelocity(0,0,omega);      //rotate to look for ball
            sendPoseMsg();
            ros::spinOnce();
            loop_rate.sleep();
        }
        
        // move toward target until it is within tolerances
        ROS_INFO("moving toward target");
        
        while(moveTowardTarget()){
            //ROS_INFO("dist to target %i,%i",distToTarget[0],distToTarget[1]);
            ros::spinOnce();
            loop_rate.sleep();
        }
        
        ROS_INFO("Target Approached. Error in pixels %i,%i",distToTarget[0],distToTarget[1]);
        ROS_INFO("Starting Pickup.");   
      
        //
        setBHAPose(0.803812,0.459433,0.498534, 0.982405, 0.694037,0.855327, false);
      
        ros::spinOnce();
        loop_rate.sleep();
      
        ros::Duration(5).sleep();
        ROS_INFO("Object Grabbed");
      
        BHAPose.gripperClosed=true;
        sendPoseMsg();
      
        ros::Duration(2).sleep();
     
        //lift up object
        ROS_INFO("lifting object"); 
        setBHAPose(0.591398, 0.522972, 0.532747, 0.796676, 0.821114, 0.904203,true);
      
        // hold it up for 10 secconds
        ros::Duration(10).sleep();
      
        ros::spinOnce();
        loop_rate.sleep();
        
    }// end while
}
</code>

The function spin starts by setting a default pose.  It then rotates to look for a target object.  When it is found it will approach the target until it is within acceptable tolerances. (the approach function is detailed later) .   Now the function moves the arm to the pickup position... waits... and closes the gripper.   After the gripper is closed the arm is lifted and and the ball is dropped again.  The function then repeats.

<code c++ moveToward.cpp>
bool approach_grab::moveTowardTarget(){
    float vx=0;
    float vy=0;
    float omega=0;
    
    // if we see the target in between the left and right tolerances we apprach the object.
    if(targetPos[0]>turnLeftTol && targetPos[0]<turnRightTol){
        // use a simple proportinoal controller to set the velocites 
        if(abs(distToTarget[1])>desiredPosError[1])  vx=distToTarget[1]*posGain;
        if(abs(distToTarget[0])>desiredPosError[0])  vy=distToTarget[0]*posGain;
    }
    
    // if the object is to the left of our tolerance zone turn left to look for it
    if(targetPos[0]<turnLeftTol){
        omega=.5;
    }
    
    // if the object is to the right of our tolerance zone turn right to look for it
    if(targetPos[0]>turnRightTol){
      omega=-.5;
    }
    
    // set the velocity of the robotino
    setRobotinoVelocity(vx,vy,omega);
    
    // we want to return ture while we are still approaching the ball.
    return (abs(distToTarget[0])>desiredPosError[0] || (distToTarget[1])>desiredPosError[1]);  // return true while we still need to move
}
</code>

First we check to see if the object is within predefined limits on the camera screen.  If it is approach the ball moving left and right to the desired position with a proportional controller.

If the ball is outside of the turn tolerances we turn left or right accordingly until we see the ball in our ranges.

now we set the velocity of the robotino to the values we computed

finally return true if we still need to approach the ball.  If we are at our desired position return false to break out of the loop in spin().

**approach_grab_node.cpp**

this file demonstrates how to use the class we developed. 

<code c++ approach_grab_node.cpp>
#include "approach_grab.h"
    
int main(int argc, char **argv)
{
   ros::init(argc, argv, "approach_grab_node");
   approach_grab mygrab;
   mygrab.spin();
   return 0;
}
</code>

all we need to do is include our header file.

initialize ros

declare an instance of our class...

... and run spin


===== Launching your project =====

to launch your project

open 4 terminal windows

terminal 1

  roslaunch robotino_node robotino_node.launch hostname:=<robotinoIP>

terminal 2

  rosrun robotino_control BHApose <robotinoIP>

terminal 3

  rosrun robotino_control vision

terminal 4

   rosrun robotino_control approach_grab_node