interactive_object_detection_frame.cpp

Go to the documentation of this file.

/*
 * Copyright (c) 2009, 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"

using namespace pr2_interactive_object_detection;

using actionlib::SimpleActionClient;


namespace pr2_interactive_object_detection {

InteractiveObjectDetectionFrame::InteractiveObjectDetectionFrame
  (InteractiveObjectDetectionDisplay *display, wxWindow *parent) :
  InteractiveObjectDetectionFrameBase(parent),
  user_cmd_action_client_("object_detection_user_command", true),
  action_requested_(false),
  root_nh_(""),
  priv_nh_("~"),
  executing_thread_(0)
{
}


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_;
  }
}

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())
  {
    Enable(false);
  }
  else
  {
    Enable( true );

    seg_button_->Enable( !action_requested_ );
    rec_button_->Enable( !action_requested_ );
    det_button_->Enable( !action_requested_ );

    cancel_button_->Enable( action_requested_ );

    status_bar_->SetLabel( wxString::FromAscii(status_.c_str()) );

    if ( !action_requested_ )
    {
      seg_status_label_->SetLabel( wxString::FromAscii(seg_status_.c_str()) );
      rec_status_label_->SetLabel( wxString::FromAscii(rec_status_.c_str()) );
      det_status_label_->SetLabel( wxString::FromAscii(det_status_.c_str()) );
    }

    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( wxCommandEvent& event )
{
  boost::mutex::scoped_lock l1( mutex_ );
  ROS_WARN( "Canceling action.." );

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


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( wxCommandEvent& event )
{
  seg_status_ = "Working..";
  rec_status_ = "";
  det_status_ = "";
  requestUserCommand( UserCommandGoal::SEGMENT, seg_interactive_cb_->IsChecked() );
}

void InteractiveObjectDetectionFrame::recButtonClicked( wxCommandEvent& event )
{
  rec_status_ = "Working..";
  det_status_ = "";
  requestUserCommand( UserCommandGoal::RECOGNIZE, rec_interactive_cb_->IsChecked() );
}

void InteractiveObjectDetectionFrame::detButtonClicked( wxCommandEvent& event )
{
  seg_status_ = "";
  rec_status_ = "";
  det_status_ = "Working..";
  requestUserCommand( UserCommandGoal::DETECT, det_interactive_cb_->IsChecked() );
}

}

---

pr2_interactive_object_detection
Author(s): David Gossow
autogenerated on Fri Jan 11 09:32:29 2013