====== Robotino Vision Pickup ======

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

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

{{youtube>29y0zNNldC0?large}}
{{youtube>hGD6ovrpo-4?large}}\\ 

===== Details of camera mounting =====

It was decided to mount the camera off to one side of the robotino so that the vision would not be obscured by the BHA (the arm).  The camera was fixed to the robotino with velcro and stabilized with tape as no mooting holes were available in the desired location.

{{dylanw:cameraright.jpeg }} {{ dylanw:cameraleft.jpeg}}

===== Setup =====

**For this tutorial you will need:**

  * an Ubuntu PC
  * ROS installed on your pc [[http://wiki.ros.org/ROS/Installation|(installation instructions)]]
  * API2 installed on your pc **and** on your robotino

**You will also need to find and install several ROS packages:**

  * [[http://wiki.ros.org/vision_opencv|vision_opencv]]  
  * [[http://wiki.openrobotino.org/index.php?title=ROS|robotino-ros-pkg]]
  * the completed ros package for this tutorial can downloaded as well {{dylanw:robotino_control.zip}}

===== Vision node =====

First we need to create a vision node to detect the location of our target object.  This package tracks the object by it's color.

The vision node is based on the following tutorial: [[http://opencv-srf.blogspot.ro/2010/09/object-detection-using-color-seperation.html|open cv color tracking]]. It is recommended that you look over this tutorial to understand the details of the vision node code.

<code c++ visonnode.cpp>
#include <ros/ros.h>
#include "std_msgs/String.h"
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>
#include "std_msgs/MultiArrayLayout.h"
#include "std_msgs/MultiArrayDimension.h"
#include "std_msgs/Int32MultiArray.h"
  
namespace enc = sensor_msgs::image_encodings;
  
static const char WINDOW[] = "Image window";
  
class ImageConverter
{
    // declare all of our node handelers
    ros::NodeHandle nh_;
    image_transport::ImageTransport it_;
    image_transport::Subscriber image_sub_;
    image_transport::Publisher image_pub_;
    ros::Publisher pos_pub_;
      
      
    public:
        //declare variables to be use later
        IplImage* imgTracking;
        int lastX;
        int lastY;
        ros::NodeHandle n;
      
        ImageConverter()
        : it_(nh_)
        {
            image_pub_ = it_.advertise("/image_processed", 1);
            pos_pub_= n.advertise<std_msgs::Int32MultiArray>("/obj_pos", 2);
            image_sub_ = it_.subscribe("/image_raw", 1, &ImageConverter::imageCb, this);
  
            cv::namedWindow(WINDOW);
        }
  
        ~ImageConverter()
        {
            cv::destroyWindow(WINDOW);
        }
      
        // same as in linked tutorial
        IplImage* GetThresholdedImage(IplImage* imgHSV){       
            IplImage* imgThresh=cvCreateImage(cvGetSize(imgHSV),IPL_DEPTH_8U, 1);
        
            // below we select the HSV values.  Thes are the values for the ball I am using on my robotino's webcam
            cvInRangeS(imgHSV, cvScalar(23,60,35), cvScalar(38,2556,256), imgThresh); 
            return imgThresh;
        }

        // same as in linked tutorial
        void trackObject(IplImage* imgThresh){

            // Calculate the moments of 'imgThresh'
            CvMoments *moments = (CvMoments*)malloc(sizeof(CvMoments));
            cvMoments(imgThresh, moments, 1);
            double moment10 = cvGetSpatialMoment(moments, 1, 0);
            double moment01 = cvGetSpatialMoment(moments, 0, 1);
            double area = cvGetCentralMoment(moments, 0, 0);
  
            // if the area<1000, I consider that the there are no object in the image and it's because of the noise, the area is not zero 
            
            if(area>100){
                // calculate the position of the ball
                int posX = moment10/area;
                int posY = moment01/area;        
  	  
                if(lastX>=0 && lastY>=0 && posX>=0 && posY>=0){
                    // Draw a yellow line from the previous point to the current point
  	            cvLine(imgTracking, cvPoint(posX, posY), cvPoint(lastX, lastY), cvScalar(0,0,255), 4);
                }
  
                lastX = posX;
                lastY = posY;
                std::cout << "X:pos" << posX <<std::endl;
                std::cout << "Y:pos" << posY <<std::endl;
  	
                // the code belosw is used to send a size 2 array to indicate the position
                // sent as rostopic /obj_pos
                std_msgs::Int32MultiArray array;
                array.data.clear();
                array.data.push_back(posX);
                array.data.push_back(posY);
                pos_pub_.publish(array);
            }
  
            free(moments); 
        }
  
        void imageCb(const sensor_msgs::ImageConstPtr& msg)
        {
            // get the pointer to the open cv image
            cv_bridge::CvImagePtr cv_ptr;
            
            try
            {
  	        cv_ptr = cv_bridge::toCvCopy(msg, enc::BGR8);
            }
            
            catch (cv_bridge::Exception& e)
            {
  	        ROS_ERROR("cv_bridge exception: %s", e.what());
  	        return;
            }
  
            if (cv_ptr->image.rows > 60 && cv_ptr->image.cols > 60){
                IplImage* frame=new IplImage(cv_ptr->image);  // in our case the source of the image is different
  	        // than that of the tutorial so we and to select frame from our cv_pointer we got from our open cv link
  	        imgTracking=cvCreateImage(cvGetSize(frame),IPL_DEPTH_8U, 3);
  	        cvZero(imgTracking); //covert the image, 'imgTracking' to black
  		    
  	        frame=cvCloneImage(frame); 
  	      
  	        cvSmooth(frame, frame, CV_GAUSSIAN,3,3); //smooth the original image using Gaussian kernel
  
  	        IplImage* imgHSV = cvCreateImage(cvGetSize(frame), IPL_DEPTH_8U, 3); 
  	        cvCvtColor(frame, imgHSV, CV_BGR2HSV); //Change the color format from BGR to HSV
  	        IplImage* imgThresh = GetThresholdedImage(imgHSV);
  	    
  	        cvSmooth(imgThresh, imgThresh, CV_GAUSSIAN,3,3); //smooth the binary image using Gaussian kernel
  	      
  	        //track the possition of the ball
  	        trackObject(imgThresh);
  
  	        // Add the tracking image and the frame
  	        cvAdd(frame, imgTracking, frame);
  
  	        cvShowImage("Ball", imgThresh);           
  	        cvShowImage("Video", frame);
  	    
  	        //Clean up used images
  	        cvReleaseImage(&frame);  // we created a pointer so we need to clean it up before we 
  				      // exit the scope of the if statement
  	        cvReleaseImage(&imgHSV);
  	        cvReleaseImage(&imgThresh);            
  	        cvReleaseImage(&frame);
            }
            
            cv::imshow(WINDOW, cv_ptr->image);
            cv::waitKey(3);
        
            image_pub_.publish(cv_ptr->toImageMsg());
        }
};
  
int main(int argc, char** argv)
{
    ros::init(argc, argv, "vision");
    ImageConverter ic;
    ros::spin();
    return 0;
}
</code>

===== BHApose =====

BHApose is a simple extension of the poser code found in [[robotino_arm_control|last weeks tutorial]]. The only major difference is that the pose is now determined by a rostopic callback function instead of asking the user for it at startup.  

The new callback functions can be seen near the head of the code. 

<code c++ BHApose.cpp>
#define _USE_MATH_DEFINES
#include <cmath>
#include <iostream>
#include <stdlib.h>
    
#include "ros/ros.h"
  
#include "std_msgs/String.h"
#include <sstream>
#include <vector>
#include "std_msgs/MultiArrayLayout.h"
#include "std_msgs/MultiArrayDimension.h"
#include "std_msgs/Float32MultiArray.h"
#include "rec/robotino/api2/all.h"
  
using namespace rec::robotino::api2;
  
float _rect[2]= {0,0};
bool _run = true;
    
class MyCom : public Com
{
    public:
        MyCom()
  		: Com( "rect" )
  	{
  	}
  
  	void errorEvent( const char* errorString )
  	{
  	    std::cerr << "Error: " << errorString << std::endl;
  	}
  
  	void connectedEvent()
  	{
  	    std::cout << "Connected." << std::endl;
  	}
  
  	void connectionClosedEvent()
  	{
  	    std::cout << "Connection closed." << std::endl;
  	}
  
  	void logEvent( const char* message, int level )
  	{
  	    std::cout << message << std::endl;
  	}
  
  	void pingEvent( float timeMs )
  	{
  	    //std::cout << "Ping: " << timeMs << "ms" << std::endl;
  	}
};
  
class MyBumper : public Bumper
{
    public:
  	MyBumper()
  		: bumped( false )
  	{
  	}
  
  	void bumperEvent( bool hasContact )
  	{
  	    bumped |= hasContact;
  	    std::cout << "Bumper has " << ( hasContact ? "contact" : "no contact") << std::endl;
  	}
  
  	bool bumped;
};  
  
MyCom com;
OmniDrive omniDrive;
MyBumper bumper;
CompactBHA cbha;
  
float targetValues[6] = { 0.591398, 0.522972, 0.532747, 0.796676, 0.821114, 0.904203}; 
  
bool gripperClosed=false;
  
// our calback funciton to retrieve the pose
void poseCallback(const std_msgs::Float32MultiArray::ConstPtr& array)
{
    int i = 0;
    // print all the remaining numbers
    for(std::vector<float>::const_iterator it = array->data.begin(); it != array->data.end(); ++it)
    {
        targetValues[i] = *it;
  	i++;
  	std::cout<<targetValues[i]<<std::endl;
    }
   
    return;
}
    
// gripper calback function.  Currenty just closes the gripper when the funciton is activated but it takes a string
// so that it can be extended later on.
void gripperCallback(const std_msgs::StringConstPtr& msg)
{  
    ROS_INFO("I heard: [%s]", msg->data.c_str());
    gripperClosed=true;
    ROS_INFO("gripper Closed");
}
  
void init( const std::string& hostname )
{
    // Initialize the actors
  
    // Connect
  	std::cout << "Connecting... ";
  	com.setAddress( hostname.c_str() );
  
  	com.connectToServer( true );
  
  	if( false == com.isConnected() )
  	{
  	    std::cout << std::endl << "Could not connect to " << com.address() << std::endl;
  	    rec::robotino::api2::shutdown();
  	    exit( 1 );
  	}
  
  	else
  	{
  	    std::cout << "success" << std::endl;
            cbha.setCompressorsEnabled( true );
  	}
}
  
void drive()
{
    float pressures[8] = { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f };
    float values[6] = { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
      
    const float maxPressure = 1.5f;
    float stepSize=.5;
    float lastTime=com.msecsElapsed();
    float lowerAvg;
    float upperAvg;
      
    float errors[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
    float tolerance=.02;
      
    bool inTolerance=false;
      
    ros::NodeHandle n;
    ros::Subscriber bha_sub = n.subscribe("/bha_pose", 100, poseCallback);
      
    ros::Subscriber sub = n.subscribe("/bha_gripper", 100, gripperCallback);
  
  
    while( com.isConnected() && false == bumper.value() && _run )
    {
        // update the string pot values
        cbha.stringPots( values );
          
        // move toward target
          
        pressures[0]=pressures[0]+(-values[0]+targetValues[0])*stepSize+(values[1]-targetValues[1])*stepSize+(values[2]-targetValues[2])*stepSize;
        pressures[1]=pressures[1]+(values[0]-targetValues[0])*stepSize+(-values[1]+targetValues[1])*stepSize+(values[2]-targetValues[2])*stepSize;
        pressures[2]=pressures[2]+(values[0]-targetValues[0])*stepSize+(values[1]-targetValues[1])*stepSize+(-values[2]+targetValues[2])*stepSize;
          
        lowerAvg=(pressures[0]+pressures[1]+pressures[2])/3;
          
        pressures[0]-=lowerAvg;
        pressures[1]-=lowerAvg;
        pressures[2]-=lowerAvg;
              
        pressures[3]=pressures[3]+(-values[3]+targetValues[3])*stepSize+(values[4]-targetValues[4])*stepSize+(values[5]-targetValues[5])*stepSize;
        pressures[4]=pressures[4]+(values[3]-targetValues[3])*stepSize+(-values[4]+targetValues[4])*stepSize+(values[5]-targetValues[5])*stepSize;
        pressures[5]=pressures[5]+(values[3]-targetValues[3])*stepSize+(values[4]-targetValues[4])*stepSize+(-values[5]+targetValues[5])*stepSize;
   
        upperAvg=(pressures[3]+pressures[4]+pressures[5])/3;
          
        pressures[3]-=upperAvg;
        pressures[4]-=upperAvg;
        pressures[5]-=upperAvg;
  	
  	pressures[7]=10;
          
        cbha.setPressures( pressures );
    
  	for( unsigned int i = 0; i < 6; ++i ){
            errors[i]=targetValues[i]-values[i];
            std::cout<< errors[i] << std::endl;
            if(tolerance > std::abs(errors[i])   ){
                  
                //std::cout << "in tolerance: " << i <<  std::endl;
            }
            
            else{
                break;
            }
              
            if(i==5){
                inTolerance=true;
  	        //std::cout << "all in tolerance" << std::endl;
            }
            else inTolerance=false;
              
        }
        
        if(inTolerance) std::cout << "all in tolerance" << std::endl;
        
  	else std::cout << "not in tolerance" << std::endl;
  
        if(inTolerance=true && gripperClosed){
  	    cbha.setGripperValve1( true );
            cbha.setGripperValve2( true );
  	}
  	
        // list values
        /*
        std::cout << std::endl;
          
        std::cout << "avg: " << lowerAvg << " bar" << std::endl;
          
        std::cout << std::endl;
          
        for( unsigned int i = 0; i < 8; ++i )
        {
            std::cout << "B" << i << ": " << pressures[i] << " bar" << std::endl;
        }
          
        std::cout << std::endl;
          
        for( unsigned int i = 0; i < 6; ++i )
        {
            std::cout << "String pot " << i  << ": " << values[i] << std::endl;
        }
  
        */
        // syncronize and delay
          
        ros::spinOnce();
          
        lastTime=com.msecsElapsed();
        com.processEvents();
  	rec::robotino::api2::msleep( 100 );
    }  
}
  
void destroy()
{
    com.disconnectFromServer();
}
  
int main( int argc, char **argv )
{
    ros::init(argc, argv, "BHApose");
    std::string hostname = "192.168.0.2";
   
    if( argc > 1 )
    {
  	hostname = argv[1];
    }
  
    try
    {
  	init( hostname );
  	drive();
  	destroy();
    }
    
    catch( const rec::robotino::api2::RobotinoException& e )
    {
  	std::cerr << "Com Error: " << e.what() << std::endl;
    }
    
    catch( const std::exception& e )
    {
  	std::cerr << "Error: " << e.what() << std::endl;
    }
    
    catch( ... )
    {
  	std::cerr << "Unknow Error" << std::endl;
    }
  
    rec::robotino::api2::shutdown();
}
</code>

===== Talker =====

Talker is the node that controls the other 2 nodes.  It first move the vehicle so that the target object is at a predefined location on the cameras vision.  then it tells poser to move the arm to a new the pickup location, grab it and hold it up.

<code c++ talker.cpp>
#include "ros/ros.h"

#include "std_msgs/String.h"
#include <geometry_msgs/Twist.h>
#include "robotino_msgs/BHAReadings.h"
#include "robotino_msgs/SetBHAPressures.h"
#include <sstream>
#include <vector>
#include <iostream>
#include "std_msgs/MultiArrayLayout.h"
#include "std_msgs/MultiArrayDimension.h"
#include "std_msgs/Int32MultiArray.h"
#include "std_msgs/Float32MultiArray.h"

// declare global vars
void arrayCallback(const std_msgs::Int32MultiArray::ConstPtr& array);
int Arr[2];


int main(int argc, char **argv)
{
    int targetX=135;
    int targetY=150;
    float posGain=.001;
    ros::init(argc, argv, "talker");
    ros::NodeHandle n;
    ros::Subscriber ball_sub = n.subscribe("/obj_pos", 100, arrayCallback);

    ros::Publisher chatter_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel", 1, true);
    ros::Publisher bha_pub = n.advertise<std_msgs::Float32MultiArray>("/bha_pose", 1, true);
    ros::Publisher gripper_pub = n.advertise<std_msgs::String>("/bha_gripper", 100);

    ros::Rate loop_rate(10);

    int count = 0;

    // loop one approach the ball
    while (ros::ok())
    {
        std::cout<< "x: " << abs(targetY-Arr[1]) << std::endl;
        std::cout<< "y: " << abs(targetX-Arr[0]) << std::endl;

        geometry_msgs::Twist cmd_vel_msg_;

        cmd_vel_msg_.linear.x 	= 0;
        cmd_vel_msg_.linear.y  	= 0;
        cmd_vel_msg_.angular.z 	= 0;

        // move the vehicle so that the ball is in the target location on the camera screen.
        if(Arr[0]>0)
        {
            if(abs(targetY-Arr[1])>2)  cmd_vel_msg_.linear.x 	= (targetY-Arr[1])*posGain;
            if(abs(targetX-Arr[0])>2)  cmd_vel_msg_.linear.y  	= (targetX-Arr[0])*posGain;
            std::cout<< "Vel set "<< std::endl;
        }

        // once we have reached the target location within tolerance break from the loop
        if(abs(targetY-Arr[1])<2  &&  abs(targetX-Arr[0])<5)
        {
            break;
        }

        chatter_pub.publish(cmd_vel_msg_);

        // publish the pose
        float pose[6]= { 0.591398, 0.522972, 0.532747, 0.796676, 0.821114, 0.904203}; // straight arm pose

        std_msgs::Float32MultiArray msg;
        for (int i = 0; i < 6; i++)
        {
            msg.data.push_back(pose[i]);
        }

        bha_pub.publish(msg);

        ros::spinOnce();

        loop_rate.sleep();

        ++count;
    }

// seccond step  move the arm to pickup location
    {
        // broadcast new pose
        float pose[6]= {0.803812,0.459433,0.498534, 0.982405, 0.694037,0.855327};
        std_msgs::Float32MultiArray msg;

        for (int i = 0; i < 6; i++)
        {
            msg.data.push_back(pose[i]);
        }

        bha_pub.publish(msg);



        ros::Duration(5).sleep();  // wait 5 secconds before closing gripper

        // close the gripper
        std_msgs::String gripmsg;
        std::stringstream ss;
        ss << "tolclose" << count;
        gripmsg.data = ss.str();
        gripper_pub.publish(gripmsg);

        ros::Duration(10).sleep();  // wait 5 secconds before closing gripper
    }


    // set arm back to origional position
    std::cout<< "loop3------ " << std::endl;
    while (ros::ok())
    {

        float pose[6]= { 0.591398, 0.522972, 0.532747, 0.796676, 0.821114, 0.904203};
        std_msgs::Float32MultiArray msg;
        for (int i = 0; i < 6; i++)
        {
            msg.data.push_back(pose[i]);
        }
        bha_pub.publish(msg);
        ros::spinOnce();

        loop_rate.sleep();

        ++count;
    }

    //end program
    return 0;
}


void arrayCallback(const std_msgs::Int32MultiArray::ConstPtr& array)
{

    int i = 0;
    // print all the remaining numbers
    for(std::vector<int>::const_iterator it = array->data.begin(); it != array->data.end(); ++it)
    {
        Arr[i] = *it;
        i++;
    }

    return;
}
</code>

===== Launching and configuring your system =====

First you will need to find your pose so you will need to launch poser from the [[robotino_arm_control|Robotino Arm Control]] tutorial.  In the completed project you will find the code for poser as well.

run poser

  rosrun robotino_control poser <robotinoIPaddress>

Now pose the arm in the position you want it to be in when it picks up the object and accept the pose.  Make sure you copy the pose values before you accept them.  The arm will now move to the position you set.

Now you need to find the camera position.  So launch the robotino_node your vision node

  roslaunch robotino_node robotino_node.launch hostname:=<robotinoIP>
  
  roslaunch robotino_control vision

You should see a terminal and a window similar to this one 

{{dylanw:color.png}}

Read off the x and y coordinates from the terminal window.  Save these values.  If you don't see a red dot on the image or you don't see any x and y values you need to adjust the HSV values to the object you are using in the vision code and recompile.

Now enter your pose values in the second stage of the talker program.  and enter your x and y values as target x and target y

===== 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 talker

you can see the procedure int  the following video.

{{youtube>hGD6ovrpo-4?large}}