object_detection_alg_node.cpp

Go to the documentation of this file.

#include "object_detection_alg_node.h"

#include <vector>

ObjectDetectionAlgNode::ObjectDetectionAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<ObjectDetectionAlgorithm>(), it_(this->public_node_handle_), 
  loquendo_client_("loquendo", true),
  hri_client_("hri", true),
  follow_target_client_("follow_target", true),
  cv_ptr_init(false), hri_busy(false), head_busy(false),
  current_state_(HOME_STATE)
{
        //cv_ptr = NULL;
  //init class attributes if necessary
        this->loop_rate_ = 5;//in [Hz]

  // [init publishers]
  this->target_pos_publisher_ = this->public_node_handle_.advertise<geometry_msgs::PoseStamped>("target_pos", 100);
        image_pub_ = it_.advertise("image_out", 1);
  
  // [init subscribers]
        image_sub_ = it_.subscribe("image_in", 1, &ObjectDetectionAlgNode::image_callback, this);
  
  // [init services]
  
  // [init clients]
  
  // [init action servers]
  
  // [init action clients]
  
        //other inits
        parameters.fun_print_parameters();//print library global parameters
        parameters.fun_load_detectors();
        ipl_scaledImage = cvCreateImage(cvSize(parameters.fun_get_image_width(), parameters.fun_get_image_height()), IPL_DEPTH_8U, 3);
  // set home state
  std::vector<std::string> xml(NUM_CLIENTS, std::string(""));
  xml[TTS_]       = "Let me introduce my self. I am Teebee; a robotic mobile platform, capable to recognize certain objects. Let's get started.";
  xml[LEDS_]      = "";
  xml[HEAD_]      = "";//"head_home.xml";
  xml[RIGHT_ARM_] = "";//"right_arm_greeting.xml";
  xml[LEFT_ARM_]  = "";//"left_arm_greeting.xml";
  //hriMakeActionRequest(xml);
  loquendoMakeActionRequest(xml[TTS_]);
}

ObjectDetectionAlgNode::~ObjectDetectionAlgNode(void)
{
  // [free dynamic memory]
  if (ipl_scaledImage != NULL) cvReleaseImage( &ipl_scaledImage );
}

void ObjectDetectionAlgNode::mainNodeThread(void)
{
  // [fill msg structures]

  // [fill srv structure and make request to the server]

  // [fill action structure and make request to the action server]

  // [publish messages]

        //cv::Mat mat_inImage;
        //cv::Mat mat_ScaledImage;
                
std::vector<std::string> xml(NUM_CLIENTS, std::string(""));
std::string object_tag;

        switch(this->current_state_)
        {
          case HOME_STATE: 
            if(!hri_busy)
            {
              this->current_state_=LOOK_STATE;
              sleep(3);
              // look position

xml[TTS_]       = "let's look for an object on the table.";
xml[LEDS_]      = "";
xml[HEAD_]      = "";//"head_look_down.xml";
xml[RIGHT_ARM_] = "";//"right_arm_greeting.xml";
xml[LEFT_ARM_]  = "";//"left_arm_greeting.xml";
//hriMakeActionRequest(xml);
loquendoMakeActionRequest(xml[TTS_]);
            }
            break;
          case LOOK_STATE:
            if(!hri_busy)
            {
              this->current_state_=DETECTION_STATE;
            }
            break;
          case DETECTION_STATE:

            // codi michael
            if( detectOject(object_tag) )
            {
              this->current_state_=HRI_STATE;
              // speack and home
//std::string msg = "I think, on the table there is " + object_tag;
xml[TTS_]       = "I think, on the table there is " + object_tag;
xml[LEDS_]      = "";
xml[HEAD_]      = "";//"head_greeting.xml";
xml[RIGHT_ARM_] = "";//"right_arm_greeting.xml";
xml[LEFT_ARM_]  = "";//"left_arm_greeting.xml";
//hriMakeActionRequest(xml);
ROS_WARN("I think, on the table there is %s", object_tag.c_str());
loquendoMakeActionRequest(xml[TTS_]);
            }

            break;
          case HRI_STATE:
            if(!hri_busy)
            {
              this->current_state_=HOME_STATE;
              // home position
xml[TTS_]       = "";
xml[LEDS_]      = "";
xml[HEAD_]      = "";//"head_home.xml";
xml[RIGHT_ARM_] = "";//"right_arm_greeting.xml";
xml[LEFT_ARM_]  = "";//"left_arm_greeting.xml";
//hriMakeActionRequest(xml);
//loquendoMakeActionRequest(xml[TTS_]);
            }
            break;
        
        }
}

bool ObjectDetectionAlgNode::detectOject(std::string & obj_tag)
{
  int numDetections,x1=0,x2=0,y1=0,y2=0,color;
  float score;
  char const* tag;
  cv_bridge::CvImage roscv_outImage;
  static int count=0;
  std::string tmp_tag;
  static std::string object_tag;
  bool matched=false,stable=false;
        
  this->image_mutex_.enter(); 
  bool image_ok = (cv_ptr_init);
  this->image_mutex_.exit(); 
  //if (cv_ptr!=NULL)
  if ( image_ok )
  {
          //from cv_ptr->image to IplImage
          this->image_mutex_.enter(); 
          IplImage ipl_inImage = cv_ptr->image;
          //mat_inImage = cv_ptr->image.clone();
          roscv_outImage.header = cv_ptr->header;
          roscv_outImage.encoding = cv_ptr->encoding;
          //this->image_mutex_.exit(); 
          
          //resize input image (v1)
          //IplImage* scaledImage = cvCreateImage(cvSize(parameters.fun_get_image_width(), parameters.fun_get_image_height()), IPL_DEPTH_8U, 3);
          //IplImage* ipl_scaledImage = cvCreateImage(cvSize(640, 480), IPL_DEPTH_8U, 3);
          cvResize(&ipl_inImage , ipl_scaledImage, CV_INTER_LINEAR );
          this->image_mutex_.exit(); 

          //object detection
          clsDetectionSet detections = fun_detection(ipl_scaledImage,&parameters);

          //detector results
          numDetections = detections.fun_get_numDetections();
          ROS_INFO("ObjectDetectionAlgNode::detectOject: numDetections=%d", numDetections);

          if (numDetections>0)
          {
                for (int ii=1; ii<=numDetections; ii++)
                {
                        //get current detection parameters and prints data
                        detections.detection[ii].fun_get_values(x1,y1,x2,y2,score,tag,color);
                        std::cout << "data: loaction -> [" << x1 << " " << y1 << " " << x2 << " " << y2 << "] : score -> " << score << " : tag->" << tag << std::endl;
                }
                
                if(count==0)
                {
                    detections.detection[1].fun_get_values(x1,y1,x2,y2,score,tag,color);                          
                    if( std::string(tag).find("cactus") != std::string::npos )
                      object_tag = "a cactus. Be aware not to get hurt";
                    else if( std::string(tag).find("bottle") != std::string::npos )
                      object_tag = "an empty, beer bottle. Who has drunk it?";
                    else if( std::string(tag).find("box") != std::string::npos )
                      object_tag = "an old fashioned floppy disk box";
                    else if( std::string(tag).find("car") != std::string::npos )
                      object_tag = "a small, toy car";
                    else if( std::string(tag).find("elvis") != std::string::npos )
                      object_tag = "a beautiful elvis mug. Is it from Graceland?";
                    else if( std::string(tag).find("stapler") != std::string::npos )
                      object_tag = "a blue stapler";
                    else if( std::string(tag).find("vase") != std::string::npos )
                      object_tag = "a plastic purple vase";
                    else if( std::string(tag).find("mug_color") != std::string::npos )
                      object_tag = "a colorful coffee mug";
                    else
                      object_tag = "none";
                    ROS_INFO("ObjectDetectionAlgNode::detectOject: chosen object=%s",object_tag.c_str());
                    count++;
                }
                else
                {
                  ROS_INFO("ObjectDetectionAlgNode::detectOject: count=%d tag=%s numDetections=%d",count,tag, numDetections);
                  for(int ii=1;ii<=numDetections;ii++)
                  {
                    detections.detection[ii].fun_get_values(x1,y1,x2,y2,score,tag,color);
                    if( std::string(tag).find("cactus") != std::string::npos )
                      tmp_tag = "a cactus. Be aware not to get hurt";
                    else if( std::string(tag).find("bottle") != std::string::npos )
                      tmp_tag = "an empty, beer bottle. Who has drunk it?";
                    else if( std::string(tag).find("box") != std::string::npos )
                      tmp_tag = "an old fashioned floppy disk box";
                    else if( std::string(tag).find("car") != std::string::npos )
                      tmp_tag = "a small, toy car";
                    else if( std::string(tag).find("elvis") != std::string::npos )
                      tmp_tag = "a beautiful elvis mug. Is it from Graceland?";
                    else if( std::string(tag).find("stapler") != std::string::npos )
                      tmp_tag = "a blue stapler";
                    else if( std::string(tag).find("vase") != std::string::npos )
                      tmp_tag = "a plastic purple vase";
                    else if( std::string(tag).find("mug_color") != std::string::npos )
                      tmp_tag = "a colorful coffee mug";
                    else 
                      tmp_tag = "none";
                    ROS_INFO("ObjectDetectionAlgNode::detectOject: matched tmp_tag=%s count=%d",tmp_tag.c_str(),count);
                    if(tmp_tag.compare(object_tag) == 0)
                    {
                      matched=true;
                      break;
                    }
                  }
                  if(!matched)
                  {
                    ROS_INFO("ObjectDetectionAlgNode::detectOject: NOT matched! count=%d",count);
                    count=0;
                  }
                  else
                  {
                    ROS_INFO("ObjectDetectionAlgNode::detectOject: tag=%s matched!! count=%d",tag,count);
                    count++;                        
                    if(count >= 5)
                    {
                      count = 0;
                      stable=true;
                    }
                  }
                }
                ROS_INFO("ObjectDetectionAlgNode::detectOject: tag=%s count=%d",tag,count);
          }

          //publish image with marked region corresponding to object location
          roscv_outImage.image = cv::cvarrToMat(ipl_scaledImage);
          image_pub_.publish(roscv_outImage.toImageMsg());

  }     

  ROS_INFO("ObjectDetectionAlgNode::detectOject: leaving stable=%d",stable);
  std::cout << std::endl;

  if(stable)
    obj_tag = object_tag;

  return stable;
}

/*  [subscriber callbacks] */
void ObjectDetectionAlgNode::image_callback(const sensor_msgs::ImageConstPtr& msg)
{
        //ROS_INFO("ObjectDetectionAlgNode::image_callback: New Image Received"); 
        try
        {       
                this->image_mutex_.enter();
                        cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
                        cv_ptr_init = true;
                this->image_mutex_.exit(); 
        }
        catch (cv_bridge::Exception& e)
        {
                this->image_mutex_.enter();
                        cv_ptr_init = false;
                this->image_mutex_.exit();
                ROS_ERROR("cv_bridge exception: %s", e.what());
                return;
        }
}

/*  [service callbacks] */

/*  [action callbacks] */
void ObjectDetectionAlgNode::loquendoDone(const actionlib::SimpleClientGoalState& state,  const iri_common_drivers_msgs::ttsResultConstPtr& result) 
{
hri_busy=false; 
  if( state.toString().compare("SUCCEEDED") == 0 ) 
    ROS_INFO("ObjectDetectionAlgNode::loquendoDone: Goal Achieved!"); 
  else 
    ROS_INFO("ObjectDetectionAlgNode::loquendoDone: %s", state.toString().c_str()); 

  //copy & work with requested result 
} 

void ObjectDetectionAlgNode::loquendoActive() 
{ 
  //ROS_INFO("ObjectDetectionAlgNode::loquendoActive: Goal just went active!"); 
} 

void ObjectDetectionAlgNode::loquendoFeedback(const iri_common_drivers_msgs::ttsFeedbackConstPtr& feedback) 
{ 
  //ROS_INFO("ObjectDetectionAlgNode::loquendoFeedback: Got Feedback!"); 

  bool feedback_is_ok = true; 

  //analyze feedback 
  //my_var = feedback->var; 

  //if feedback is not what expected, cancel requested goal 
  if( !feedback_is_ok ) 
  { 
    loquendo_client_.cancelGoal(); 
    //ROS_INFO("ObjectDetectionAlgNode::loquendoFeedback: Cancelling Action!"); 
  } 
}
void ObjectDetectionAlgNode::hriDone(const actionlib::SimpleClientGoalState& state,  const tibi_dabo_msgs::sequenceResultConstPtr& result) 
{ 
  hri_busy=false;
  
  if( state.toString().compare("SUCCEEDED") == 0 ) 
    ROS_INFO("ObjectDetectionAlgNode::hriDone: Goal Achieved!"); 
  else 
    ROS_INFO("ObjectDetectionAlgNode::hriDone: %s", state.toString().c_str()); 

  //copy & work with requested result 
} 

void ObjectDetectionAlgNode::hriActive() 
{ 
  //ROS_INFO("ObjectDetectionAlgNode::hriActive: Goal just went active!"); 
} 

void ObjectDetectionAlgNode::hriFeedback(const tibi_dabo_msgs::sequenceFeedbackConstPtr& feedback) 
{ 
  //ROS_INFO("ObjectDetectionAlgNode::hriFeedback: Got Feedback!"); 

  bool feedback_is_ok = true; 

  //analyze feedback 
  //my_var = feedback->var; 

  //if feedback is not what expected, cancel requested goal 
  if( !feedback_is_ok ) 
  { 
    hri_client_.cancelGoal(); 
    //ROS_INFO("ObjectDetectionAlgNode::hriFeedback: Cancelling Action!"); 
  } 
}
void ObjectDetectionAlgNode::follow_targetDone(const actionlib::SimpleClientGoalState& state,  const iri_nav_msgs::followTargetResultConstPtr& result) 
{ 
  head_busy = false;
  
  if( state.toString().compare("SUCCEEDED") == 0 ) 
    ROS_INFO("ObjectDetectionAlgNode::follow_targetDone: Goal Achieved!"); 
  else 
    ROS_INFO("ObjectDetectionAlgNode::follow_targetDone: %s", state.toString().c_str()); 

  //copy & work with requested result 
} 

void ObjectDetectionAlgNode::follow_targetActive() 
{ 
  //ROS_INFO("ObjectDetectionAlgNode::follow_targetActive: Goal just went active!"); 
} 

void ObjectDetectionAlgNode::follow_targetFeedback(const iri_nav_msgs::followTargetFeedbackConstPtr& feedback) 
{ 
  //ROS_INFO("ObjectDetectionAlgNode::follow_targetFeedback: Got Feedback!"); 

  bool feedback_is_ok = true; 

  //analyze feedback 
  //my_var = feedback->var; 
  if (feedback->current_dist < 2.0)
  {
    feedback_is_ok = false; //force to cancel action
    ROS_INFO("Head Action complete!");
  }

  //if feedback is not what expected, cancel requested goal 
  if( !feedback_is_ok ) 
  { 
    follow_target_client_.cancelGoal(); 
    //ROS_INFO("ObjectDetectionAlgNode::follow_targetFeedback: Cancelling Action!"); 
  } 
}

/*  [action requests] */
void ObjectDetectionAlgNode::loquendoMakeActionRequest(const std::string & msg) 
{ 
  ROS_INFO("ObjectDetectionAlgNode::loquendoMakeActionRequest: Starting New Request!"); 

  //wait for the action server to start 
  //will wait for infinite time 
  loquendo_client_.waitForServer();  
  ROS_INFO("ObjectDetectionAlgNode::loquendoMakeActionRequest: Server is Available!"); 

  //send a goal to the action 
  loquendo_goal_.msg = msg;
  loquendo_client_.sendGoal(loquendo_goal_, 
              boost::bind(&ObjectDetectionAlgNode::loquendoDone,     this, _1, _2), 
              boost::bind(&ObjectDetectionAlgNode::loquendoActive,   this), 
              boost::bind(&ObjectDetectionAlgNode::loquendoFeedback, this, _1)); 
  ROS_INFO("ObjectDetectionAlgNode::loquendoMakeActionRequest: Goal Sent. Wait for Result!");
hri_busy=true;
}
void ObjectDetectionAlgNode::hriMakeActionRequest(const std::vector<std::string> & xml_files) 
{ 
  ROS_INFO("ObjectDetectionAlgNode::hriMakeActionRequest: Starting New Request!"); 

  //wait for the action server to start 
  //will wait for infinite time 
  hri_client_.waitForServer();  
  ROS_INFO("ObjectDetectionAlgNode::hriMakeActionRequest: Server is Available!"); 

  tibi_dabo_msgs::sequenceGoal hri_goal;
  hri_goal.sequence_file = std::vector<std::string>(NUM_CLIENTS, std::string(""));
  for(unsigned int ii=0; ii<NUM_CLIENTS; ii++)
    hri_goal.sequence_file[ii] = xml_files[ii];

  //send a goal to the action 
  //hri_goal_.data = my_desired_goal; 
  hri_client_.sendGoal(hri_goal, 
              boost::bind(&ObjectDetectionAlgNode::hriDone,     this, _1, _2), 
              boost::bind(&ObjectDetectionAlgNode::hriActive,   this), 
              boost::bind(&ObjectDetectionAlgNode::hriFeedback, this, _1)); 
  ROS_INFO("ObjectDetectionAlgNode::hriMakeActionRequest: Goal Sent. Wait for Result!"); 
  
  hri_busy=true;
}
void ObjectDetectionAlgNode::follow_targetMakeActionRequest() 
{ 
  ROS_INFO("ObjectDetectionAlgNode::follow_targetMakeActionRequest: Starting New Request!"); 

  //wait for the action server to start 
  //will wait for infinite time 
  follow_target_client_.waitForServer();  
  ROS_INFO("ObjectDetectionAlgNode::follow_targetMakeActionRequest: Server is Available!"); 

  //send a goal to the action 
  //follow_target_goal_.data = my_desired_goal; 
  follow_target_client_.sendGoal(follow_target_goal_, 
              boost::bind(&ObjectDetectionAlgNode::follow_targetDone,     this, _1, _2), 
              boost::bind(&ObjectDetectionAlgNode::follow_targetActive,   this), 
              boost::bind(&ObjectDetectionAlgNode::follow_targetFeedback, this, _1)); 
  ROS_INFO("ObjectDetectionAlgNode::follow_targetMakeActionRequest: Goal Sent. Wait for Result!"); 
}

void ObjectDetectionAlgNode::node_config_update(Config &config, uint32_t level)
{
  this->alg_.lock();

  this->alg_.unlock();
}

void ObjectDetectionAlgNode::addNodeDiagnostics(void)
{
}

/* main function */
int main(int argc,char *argv[])
{
  return algorithm_base::main<ObjectDetectionAlgNode>(argc, argv, "object_detection_alg_node");
}