interactive_object_detection_frame.cpp

Go to the documentation of this file.

/*
 * Copyright (c) 2012, Willow Garage, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Willow Garage, Inc. nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include "pr2_interactive_object_detection/interactive_object_detection_frame.h"

#include "ui_main_frame.h" // generated by uic during build.

using namespace pr2_interactive_object_detection;

using actionlib::SimpleActionClient;


namespace pr2_interactive_object_detection {

InteractiveObjectDetectionFrame::InteractiveObjectDetectionFrame( QWidget *parent )
  : QWidget( parent )
  , user_cmd_action_client_("object_detection_user_command", true)
  , action_requested_(false)
  , root_nh_("")
  , priv_nh_("~")
  , executing_thread_(0)
  , ui_( new Ui::MainFrame )
{
  ui_->setupUi( this );
  connect( ui_->seg_button_, SIGNAL( clicked() ), this, SLOT( segButtonClicked() ));
  connect( ui_->rec_button_, SIGNAL( clicked() ), this, SLOT( recButtonClicked() ));
  connect( ui_->det_button_, SIGNAL( clicked() ), this, SLOT( detButtonClicked() ));
  connect( ui_->cancel_button_, SIGNAL( clicked() ), this, SLOT( cancelButtonClicked() ));
}

InteractiveObjectDetectionFrame::~InteractiveObjectDetectionFrame()
{
  if (executing_thread_)
  {
    if (!mutex_.try_lock())
    {
      ROS_INFO("Waiting for execution thread to finish");
      mutex_.lock();
      ROS_INFO("Execution thread finished.");
    }
    mutex_.unlock();
    executing_thread_->join();
    delete executing_thread_;
  }

  delete ui_;
}

void InteractiveObjectDetectionFrame::requestUserCommand( int8_t request, bool interactive )
{
  if (!mutex_.try_lock())
  {
    ROS_WARN( "User command already executing. Action canceled." );
    return;
  }
  mutex_.unlock();
  if (executing_thread_ != 0)
  {
    executing_thread_->join();
    delete executing_thread_;
  }

  boost::function< void () > user_command =
      boost::bind( &InteractiveObjectDetectionFrame::executeRequest, this, request, interactive );

  executing_thread_ = new boost::thread( user_command );
}


void InteractiveObjectDetectionFrame::update()
{
  if (!mutex_.try_lock())
  {
    setEnabled( false );
  }
  else
  {
    setEnabled( true );

    ui_->seg_button_->setEnabled( !action_requested_ );
    ui_->rec_button_->setEnabled( !action_requested_ );
    ui_->det_button_->setEnabled( !action_requested_ );

    ui_->cancel_button_->setEnabled( action_requested_ );

    ui_->status_bar_->setText( QString::fromStdString( status_ ));

    if ( !action_requested_ )
    {
      ui_->seg_status_label_->setText( QString::fromStdString( seg_status_ ));
      ui_->rec_status_label_->setText( QString::fromStdString( rec_status_ ));
      ui_->det_status_label_->setText( QString::fromStdString( det_status_ ));
    }

    mutex_.unlock();
  }
}


void InteractiveObjectDetectionFrame::userCmdDone(const actionlib::SimpleClientGoalState& state,
                                                  const UserCommandResultConstPtr& result)
{
  boost::mutex::scoped_lock l1( mutex_ );

  ROS_INFO("Action finished in state [%s]", state.toString().c_str());
  action_requested_ = false;
  status_ = "[" + state.toString() + "]";

  switch (user_command_goal_.request)
  {
    case UserCommandGoal::SEGMENT:
      seg_status_ = state.getText();
      break;
    case UserCommandGoal::RECOGNIZE:
      rec_status_ = state.getText();
      break;
    case UserCommandGoal::DETECT:
      det_status_ = state.getText();
      break;    
    default:
      break;
  }
}


// Called once when the goal becomes active
void InteractiveObjectDetectionFrame::userCmdActive()
{
  boost::mutex::scoped_lock l1( mutex_ );
}


// Called every time feedback is received for the goal
void InteractiveObjectDetectionFrame::userCmdFeedback(const UserCommandFeedbackConstPtr& feedback)
{
  boost::mutex::scoped_lock l1( mutex_ );
  if ( action_requested_ )
  {
    ROS_INFO_STREAM( feedback->status );
    status_ = feedback->status;
  }
}


void InteractiveObjectDetectionFrame::cancelButtonClicked()
{
  boost::mutex::scoped_lock l1( mutex_ );
  ROS_WARN( "Canceling action.." );

  user_cmd_action_client_.cancelGoal();
  ROS_INFO( "Action canceled." );
}


void InteractiveObjectDetectionFrame::resetButtonClicked()
{
  seg_status_ = "";
  rec_status_ = "";
  det_status_ = "";
  requestUserCommand( UserCommandGoal::RESET, 0 );  
  ROS_INFO( "Detection markers reset." );
}


void InteractiveObjectDetectionFrame::executeRequest( int8_t request, bool interactive )
{
  boost::mutex::scoped_lock l1( mutex_ );

  if ( action_requested_ )
  {
    ROS_ERROR( "Action is currently running. Cannot start new one." );
    return;
  }

  if ( !user_cmd_action_client_.waitForServer( ros::Duration(2.0) ) )
  {
    status_ = "ERROR: Action server not found.";
    return;
  }

  status_ = "Action requested.";

  ROS_INFO( "Requesting user command %d, interactive =%d", request, interactive );
  action_requested_ = true;

  user_command_goal_.request = request;
  user_command_goal_.interactive = interactive;

  user_cmd_action_client_.sendGoal(user_command_goal_,
      boost::bind(&InteractiveObjectDetectionFrame::userCmdDone, this, _1, _2),
      boost::bind(&InteractiveObjectDetectionFrame::userCmdActive, this),
      boost::bind(&InteractiveObjectDetectionFrame::userCmdFeedback, this, _1) );
}


void InteractiveObjectDetectionFrame::segButtonClicked()
{
  seg_status_ = "Working..";
  rec_status_ = "";
  det_status_ = "";
  requestUserCommand( UserCommandGoal::SEGMENT, ui_->interactive_cb_->isChecked() );
}

void InteractiveObjectDetectionFrame::recButtonClicked()
{
  rec_status_ = "Working..";
  det_status_ = "";
  requestUserCommand( UserCommandGoal::RECOGNIZE, ui_->interactive_cb_->isChecked() );
}

void InteractiveObjectDetectionFrame::detButtonClicked()
{
  seg_status_ = "";
  rec_status_ = "";
  det_status_ = "Working..";
  requestUserCommand( UserCommandGoal::DETECT, ui_->interactive_cb_->isChecked() );
}

}