GazeboRosRfidTag.cpp

/* Copyright (C)
 *     Jonas Mellin & Zakiruz Zaman
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */
/* Desc: RFID Tag
 * Author: Jonas Mellin & Zakiruz Zaman
 * Date: 6th December 2011
 */

#include <gazebo/msgs/msgs.hh>

#include <map>
#include <string>
#include <gazebo/sensors/SensorTypes.hh>
#include <gazebo/sensors/Sensor.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/math/Vector3.hh>
#include <ros/ros.h>

#include <gazebo_plugins/GazeboRosRfidTag.h>
#include <gazebo_plugins/GazeboRosRfid.h>

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

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

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

  delete this->rosnode_;
}

/////////////////////////////////////////////////
void GazeboRosRfidTag::Load( physics::ModelPtr _parent, sdf::ElementPtr _sdf )
{
  this->parentSensor = boost::dynamic_pointer_cast<sensors::RFIDTag>(_parent);
  if (!this->parentSensor)
  {
    ROS_ERROR("Rfid tag parent is not of type RFIDTag");
    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>();

   // 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_);
  // Add the tag to all the RFID sensors.
  Sensor_V sensors = SensorManager::Instance()->GetSensors();
  for (Sensor_V::iterator iter = sensors.begin(); iter != sensors.end(); ++iter)
  {
    if ((*iter)->GetType() == "rfid")
    {
      boost::shared_dynamic_cast<GazeboRosRfid>(*iter)->AddTag(this);
    }
  }

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

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

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

//////////////////////////////////////////////////
void GazeboRosRfidTag::OnScan()
{
  if (this->scanPub)
  {
    msgs::Pose msg;
    msgs::Set(&msg, entity->GetWorldPose());

    // msg.set_position(link->GetWorldPose().pos);
    // msg.set_orientation(link->GetWorldPose().rot);
    // msgs::LaserScan msg;

    // msg.set_frame(this->link->GetScopedName());
    // msgs::Set(msg.mutable_offset(), this->GetPose());
    // msg.set_angle_min( this->GetAngleMin().Radian() );
    // msg.set_angle_max( this->GetAngleMax().Radian() );
    // msg.set_angle_step( this->GetAngleResolution() );

    // msg.set_range_min( this->GetRangeMin() );
    // msg.set_range_max( this->GetRangeMax() );

    // for (unsigned int i=0; i < (unsigned int)this->GetRangeCount(); i++)
    // {
    //   msg.add_ranges(this->laserShape->GetRange(i));
    //   msg.add_intensities(0);
    // }

    this->scanPub->Publish(msg);
    // std::cout << "update impl for rfidtag called" << std::endl;
  }
}