gazebo_ros_grasp_hack.cpp

Go to the documentation of this file.

/*
 *  Gazebo - Outdoor Multi-Robot Simulator
 *  Copyright (C) 2003
 *     Nate Koenig & Andrew Howard
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */
/*
 * Desc: 3D position interface for ground truth.
 * Author: Sachin Chitta and John Hsu
 * Date: 1 June 2008
 * SVN info: $Id$
 */

#include <pr2_arm_gazebo/gazebo_ros_grasp_hack.h>

#include <gazebo/Global.hh>
#include <gazebo/XMLConfig.hh>
#include <gazebo/HingeJoint.hh>
#include <gazebo/SliderJoint.hh>
#include <gazebo/Simulator.hh>
#include <gazebo/gazebo.h>
#include <gazebo/GazeboError.hh>
#include <gazebo/ControllerFactory.hh>
#include <gazebo/World.hh>

using namespace gazebo;

GZ_REGISTER_DYNAMIC_CONTROLLER("gazebo_ros_grasp_hack", GazeboRosGraspHack);

// Constructor
GazeboRosGraspHack::GazeboRosGraspHack(Entity *parent )
   : Controller(parent)
{
  ROS_ERROR("starting grasp hack");

  // check for contact with [lr]_gripper_*_finger_tip_link
  // if contacts exist, grab relative pose between this body and the [lr]_gripper_palm_link
  // fix object to the relative pose until we lose contact

  this->myParentBody = dynamic_cast<Body*>(this->parent);

  if (!this->myParentBody)
    gzthrow("GazeboRosGraspHack controller requires a Body as its parent");

  this->myParentModel = myParentBody->GetModel();

  Param::Begin(&this->parameters);
  this->robotNamespaceP = new ParamT<std::string>("robotNamespace", "/", 0);
  this->topicNameP = new ParamT<std::string>("topicName", "grasp_hack", 0);
  Param::End();

  // danger: assume start up without grasp
  this->l_grasp = false;
  this->r_grasp = false;
}

// Destructor
GazeboRosGraspHack::~GazeboRosGraspHack()
{
  delete this->robotNamespaceP;
  delete this->topicNameP;
  delete this->rosnode_;
}

// Load the controller
void GazeboRosGraspHack::LoadChild(XMLConfigNode *node)
{
  this->robotNamespaceP->Load(node);
  this->robotNamespace = this->robotNamespaceP->GetValue();
  if (!ros::isInitialized())
  {
    int argc = 0;
    char** argv = NULL;
    ros::init(argc,argv,"gazebo",ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
  }

  this->rosnode_ = new ros::NodeHandle(this->robotNamespace);

  this->topicNameP->Load(node);
  this->topicName = this->topicNameP->GetValue();

  if (this->topicName != "")
  {
  ros::AdvertiseOptions grasp_hack_ao = ros::AdvertiseOptions::create<std_msgs::String>(
    this->topicName,1,
    boost::bind( &GazeboRosGraspHack::GraspHackConnect,this),
    boost::bind( &GazeboRosGraspHack::GraspHackDisconnect,this), ros::VoidPtr(), &this->grasp_hack_queue_);
  this->pub_ = this->rosnode_->advertise(grasp_hack_ao);
  }


}

// Increment count
void GazeboRosGraspHack::GraspHackConnect()
{
  this->graspHackConnectCount++;
}
// Decrement count
void GazeboRosGraspHack::GraspHackDisconnect()
{
  this->graspHackConnectCount--;
}

// Initialize the controller
void GazeboRosGraspHack::InitChild()
{
  this->last_time = Simulator::Instance()->GetSimTime();

  // start custom queue for grasp_hack
  this->callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosGraspHack::GraspHackQueueThread,this ) );

  std::vector<Model*> all_models = World::Instance()->GetModels();
  for (std::vector<Model*>::iterator iter = all_models.begin(); iter != all_models.end(); iter++)
  {
    if (*iter)
    {
      this->l_gripper_palm_body          = dynamic_cast<Body*>((*iter)->GetBody("l_wrist_roll_link"));
      this->r_gripper_palm_body          = dynamic_cast<Body*>((*iter)->GetBody("r_wrist_roll_link"));
      this->l_gripper_l_finger_tip_body  = dynamic_cast<Body*>((*iter)->GetBody("l_gripper_l_finger_tip_link"));
      this->l_gripper_r_finger_tip_body  = dynamic_cast<Body*>((*iter)->GetBody("l_gripper_r_finger_tip_link"));
      this->r_gripper_l_finger_tip_body  = dynamic_cast<Body*>((*iter)->GetBody("r_gripper_l_finger_tip_link"));
      this->r_gripper_r_finger_tip_body  = dynamic_cast<Body*>((*iter)->GetBody("r_gripper_r_finger_tip_link"));

      this->l_gripper_l_finger_joint     = (*iter)->GetJoint("l_gripper_l_finger_joint");
      this->l_gripper_r_finger_joint     = (*iter)->GetJoint("l_gripper_r_finger_joint");
      this->l_gripper_l_finger_tip_joint = (*iter)->GetJoint("l_gripper_l_finger_tip_joint");
      this->l_gripper_r_finger_tip_joint = (*iter)->GetJoint("l_gripper_r_finger_tip_joint");

      this->r_gripper_l_finger_joint     = (*iter)->GetJoint("r_gripper_l_finger_joint");
      this->r_gripper_r_finger_joint     = (*iter)->GetJoint("r_gripper_r_finger_joint");
      this->r_gripper_l_finger_tip_joint = (*iter)->GetJoint("r_gripper_l_finger_tip_joint");
      this->r_gripper_r_finger_tip_joint = (*iter)->GetJoint("r_gripper_r_finger_tip_joint");
    }
  }
  if (!this->r_gripper_palm_body || !this->r_gripper_r_finger_tip_body)
    gzthrow("GazeboRosGraspHack controller could not find r_gripper_palm_body or r_gripper_r_finger_tip_body");
}

// Update the controller
void GazeboRosGraspHack::UpdateChild()
{
  //ROS_ERROR("jc %d",gazebo::World::Instance()->GetContactJointCount());

  if (l_grasp)
  {
    // fix myParentBody relative to l_gripper_palm_body
    gazebo::Pose3d pose = this->l_grasp_relative_pose + this->l_gripper_palm_body->GetWorldPose();
    this->myParentModel->SetWorldPose(pose);
  }

  //check for contact with left gripper finger tips
  {
    bool l_gripper_l_finger = false;
    bool l_gripper_r_finger = false;

    int jointCount = gazebo::World::Instance()->GetContactJointCount();
    for (int i = 0; i < jointCount ; i++)
    {
      World::ContactJoint j = gazebo::World::Instance()->GetContactJoint(i);
      if ((j.b1 == this->myParentBody) || (j.b2 == this->myParentBody))
      {
        if ((j.b1 == this->l_gripper_l_finger_tip_body) || (j.b2 == this->l_gripper_l_finger_tip_body))
          l_gripper_l_finger = true;
        if ((j.b1 == this->l_gripper_r_finger_tip_body) || (j.b2 == this->l_gripper_r_finger_tip_body))
          l_gripper_r_finger = true;
      }
    }

    if (l_gripper_l_finger && l_gripper_r_finger)
    {
      if (!l_grasp)
      {
        l_grasp = true;
        l_grasp_relative_pose = this->myParentModel->GetWorldPose() - this->l_gripper_palm_body->GetWorldPose();
      }
    }
    else
      l_grasp = false;
  }

  if (r_grasp)
  {
    // fix myParentBody relative to r_gripper_palm_body
    gazebo::Pose3d pose = this->r_grasp_relative_pose + this->r_gripper_palm_body->GetWorldPose();
    this->myParentModel->SetWorldPose(pose);
    //std::cout << "right grasp hack active [" << pose << "]\n";

    // migrate setModelConfiguration from gazeboros into gazebo trunk, and we can do this
    //this->r_gripper_l_finger_joint_position     = this->r_gripper_l_finger_joint->GetAngle(0).GetAsRadian();
    //this->r_gripper_r_finger_joint_position     = this->r_gripper_r_finger_joint->GetAngle(0).GetAsRadian();
    //this->r_gripper_l_finger_tip_joint_position = this->r_gripper_l_finger_tip_joint->GetAngle(0).GetAsRadian();
    //this->r_gripper_r_finger_tip_joint_position = this->r_gripper_r_finger_tip_joint->GetAngle(0).GetAsRadian();
  }


  //check for contact with right gripper finger tips
  {
    bool r_gripper_l_finger = false;
    bool r_gripper_r_finger = false;

    int jointCount = gazebo::World::Instance()->GetContactJointCount();
    for (int i = 0; i < jointCount ; i++)
    {
      World::ContactJoint j = gazebo::World::Instance()->GetContactJoint(i);
      //ROS_ERROR("b %s b1 %s b2 %s",this->myParentBody->GetName().c_str(),j.b1->GetName().c_str(),j.b2->GetName().c_str());
      if ((j.b1 == this->myParentBody) || (j.b2 == this->myParentBody))
      {
        if ((j.b1 == this->r_gripper_l_finger_tip_body) || (j.b2 == this->r_gripper_l_finger_tip_body))
          r_gripper_l_finger = true;
        if ((j.b1 == this->r_gripper_r_finger_tip_body) || (j.b2 == this->r_gripper_r_finger_tip_body))
          r_gripper_r_finger = true;
        //ROS_ERROR("%d %d b %ld b1 %ld b2 %ld rtip %ld ltip %ld",(int)r_gripper_l_finger,(int)r_gripper_r_finger,(long)this->myParentBody,(long)j.b1,(long)j.b2,(long)this->r_gripper_r_finger_tip_body,(long)this->r_gripper_l_finger_tip_body);
      }
      if (r_gripper_l_finger && r_gripper_r_finger)
      {
        if (!r_grasp)
        {
          this->r_grasp = true;
          this->r_grasp_relative_pose = this->myParentModel->GetWorldPose() - this->r_gripper_palm_body->GetWorldPose();
          this->r_gripper_l_finger_joint_position = this->r_gripper_l_finger_joint->GetAngle(0).GetAsRadian();
          this->r_gripper_r_finger_joint_position = this->r_gripper_r_finger_joint->GetAngle(0).GetAsRadian();
          this->r_gripper_l_finger_tip_joint_position = this->r_gripper_l_finger_tip_joint->GetAngle(0).GetAsRadian();
          this->r_gripper_r_finger_tip_joint_position = this->r_gripper_r_finger_tip_joint->GetAngle(0).GetAsRadian();

          break;
        }
      }
    }
    if (!r_gripper_l_finger || !r_gripper_r_finger)
      r_grasp = false;
  }


  {
    gazebo::Time cur_time = Simulator::Instance()->GetSimTime();
    // get inertial Rates
    // differentiate to get accelerations
    double tmp_dt = cur_time.Double() - this->last_time.Double();
    if (tmp_dt != 0)
    {
      this->lock.lock();
      if (this->topicName != "")
      {
        std_msgs::String pubStr;
        pubStr.data = "ok";
        // publish to ros
        this->pub_.publish(pubStr);
      }
      this->lock.unlock();

      // save last time stamp
      this->last_time = cur_time;
    }
  }
}

// Finalize the controller
void GazeboRosGraspHack::FiniChild()
{
  this->rosnode_->shutdown();
  this->grasp_hack_queue_.clear();
  this->grasp_hack_queue_.disable();
  this->callback_queue_thread_.join();
}

// Put laser data to the interface
void GazeboRosGraspHack::GraspHackQueueThread()
{
  static const double timeout = 0.01;

  while (this->rosnode_->ok())
  {
    this->grasp_hack_queue_.callAvailable(ros::WallDuration(timeout));
  }
}