GazeboRosRfid.cpp

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2013, Open Source Robotics Foundation
 *  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 Open Source Robotics Foundation
 *     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.
 *********************************************************************/

/**
 *  \author Dave Coleman
 *  \desc   Example ROS plugin for Gazebo
 */

#include <map>
#include <string>
#include <gazebo/sensors/SensorTypes.hh>
#include <gazebo/sensors/Sensor.hh>
#include <gazebo/physics/HingeJoint.hh>
#include <gazebo/physics/Contact.hh>
#include <sdf/sdf.hh>
#include <sdf/Param.hh>
#include <gazebo/math/Pose.hh>
#include <gazebo/math/Quaternion.hh>
#include <tf/tf.h>


#include <gazebo/physics/World.hh>
#include <gazebo/physics/Entity.hh>

#include <gazebo/common/Exception.hh>

#include <gazebo/transport/Node.hh>
#include <gazebo/transport/Publisher.hh>

#include <gazebo/msgs/msgs.hh>

#include <gazebo/math/Vector3.hh>
#include <gazebo_plugins/GazeboRosRfidTag.h>
#include <gazebo_plugins/GazeboRosRfid.h>
#include <ros/ros.h>

namespace gazebo
{
// Register this plugin with the simulator
GZ_REGISTER_MODEL_PLUGIN(GazeboRosRfid);

////////////////////////////////////////////////////////////////////////////////
// Constructor
GazeboRosRfid::GazeboRosRfid() : SensorPlugin()
{
}

////////////////////////////////////////////////////////////////////////////////
// Destructor
GazeboRosRfid::~GazeboRosRfid()
{
  this->rosnode_->shutdown();
  this->callback_queue_thread_.join();

  delete this->rosnode_;
}

////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboRosRfid::Load( physics::ModelPtr _parent, sdf::ElementPtr _sdf )
{
  this->parentSensor = boost::dynamic_pointer_cast<sensors::RFIDSensor>(_parent);
  if (!this->parentSensor)
  {
    ROS_ERROR("Rfid sensor parent is not of type RFIDSensor");
    return;
  }

  this->robot_namespace_ = "";
  if (_sdf->HasElement("robotNamespace"))
    this->robot_namespace_ =
      _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";

  // "publishing contact/collisions to this topic name: "
  // << this->bumper_topic_name_ << std::endl;
  this->scan_topic_name_ = "rfidscan_poses";
  if (_sdf->GetElement("scanTopicName"))
    this->scan_topic_name_ =
      _sdf->GetElement("scanTopicName")->Get<std::string>();


 // "transform contact/collisions pose, forces to this body (link) name: "
  // << this->frame_name_ << std::endl;
  if (!_sdf->HasElement("frameName"))
  {
    ROS_INFO("scan plugin missing <frameName>, defaults to world");
    this->frame_name_ = "world";
  }
  else
    this->frame_name_ = _sdf->GetElement("frameName")->Get<std::string>();

  // Make sure the ROS node for Gazebo has already been initalized
  if (!ros::isInitialized())
  {
    ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
      << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
    return;
  }

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


  // resolve tf prefix
  std::string prefix;
  this->rosnode_->getParam(std::string("tf_prefix"), prefix);
  this->frame_name_ = tf::resolve(prefix, this->frame_name_);

  this->pose_pub_ = this->rosnode_->advertise<msgs::Pose>(
    std::string(this->scan_topic_name_), 1);

  // Initialize
  // start custom queue for contact bumper
  this->callback_queue_thread_ = boost::thread(
      boost::bind(&GazeboRosRfid::ScanQueueThread, this));

  // Listen to the update event. This event is broadcast every
  // simulation iteration.
  this->update_connection_ = this->parentSensor->ConnectUpdated(
     boost::bind(&GazeboRosRfid::OnScan, this));

  // Make sure the parent sensor is active.
  this->parentSensor->SetActive(true);








}

////////////////////////////////////////////////////////////////////////////////
// Update the controller
void GazeboRosRfid::OnScan()
{
  this->EvaluateTags();
  this->lastMeasurementTime = this->world->GetSimTime();

  if (this->scanPub)
  {
    msgs::Pose msg;
    msgs::Set(&msg, this->entity->GetWorldPose());
    this->scanPub->Publish(msg);
  }

  return;
}

void GazeboRosRfid::EvaluateTags()
{
  std::vector<RFIDTag*>::const_iterator ci;

  // iterate through the tags contained given rfid tag manager
  for (ci = this->tags.begin(); ci != this->tags.end(); ++ci)
  {
    math::Pose pos = (*ci)->GetTagPose();
    // std::cout << "link: " << tagModelPtr->GetName() << std::endl;
    // std::cout << "link pos: x" << pos.pos.x
    //     << " y:" << pos.pos.y
    //     << " z:" << pos.pos.z << std::endl;
    this->CheckTagRange(pos);
  }
}

bool GazeboRosRfid::CheckTagRange(const math::Pose &_pose)
{
  // copy sensor vector pos into a temp var
  math::Vector3 v;
  v = _pose.pos - this->entity->GetWorldPose().pos;

  // std::cout << v.GetLength() << std::endl;

  if (v.GetLength() <= 5.0)
  {
    // std::cout << "detected " <<  v.GetLength() << std::endl;
    return true;
  }

  // this->CheckRayIntersection(link);
  return false;
}

void RFIDSensor::AddTag(RFIDTag *_tag)
{
  this->tags.push_back(_tag);
}

}