Advertisement
Guest User

GazeboRosRfid.cpp

a guest
May 8th, 2014
592
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 6.76 KB | None | 0 0
  1. /*********************************************************************
  2.  * Software License Agreement (BSD License)
  3.  *
  4.  *  Copyright (c) 2013, Open Source Robotics Foundation
  5.  *  All rights reserved.
  6.  *
  7.  *  Redistribution and use in source and binary forms, with or without
  8.  *  modification, are permitted provided that the following conditions
  9.  *  are met:
  10.  *
  11.  *   * Redistributions of source code must retain the above copyright
  12.  *     notice, this list of conditions and the following disclaimer.
  13.  *   * Redistributions in binary form must reproduce the above
  14.  *     copyright notice, this list of conditions and the following
  15.  *     disclaimer in the documentation and/or other materials provided
  16.  *     with the distribution.
  17.  *   * Neither the name of the Open Source Robotics Foundation
  18.  *     nor the names of its contributors may be
  19.  *     used to endorse or promote products derived
  20.  *     from this software without specific prior written permission.
  21.  *
  22.  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  23.  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  24.  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
  25.  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
  26.  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  27.  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
  28.  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  29.  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  30.  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  31.  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
  32.  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  33.  *  POSSIBILITY OF SUCH DAMAGE.
  34.  *********************************************************************/
  35.  
  36. /**
  37.  *  \author Dave Coleman
  38.  *  \desc   Example ROS plugin for Gazebo
  39.  */
  40.  
  41. #include <map>
  42. #include <string>
  43. #include <gazebo/sensors/SensorTypes.hh>
  44. #include <gazebo/sensors/Sensor.hh>
  45. #include <gazebo/physics/HingeJoint.hh>
  46. #include <gazebo/physics/Contact.hh>
  47. #include <sdf/sdf.hh>
  48. #include <sdf/Param.hh>
  49. #include <gazebo/math/Pose.hh>
  50. #include <gazebo/math/Quaternion.hh>
  51. #include <tf/tf.h>
  52.  
  53.  
  54. #include <gazebo/physics/World.hh>
  55. #include <gazebo/physics/Entity.hh>
  56.  
  57. #include <gazebo/common/Exception.hh>
  58.  
  59. #include <gazebo/transport/Node.hh>
  60. #include <gazebo/transport/Publisher.hh>
  61.  
  62. #include <gazebo/msgs/msgs.hh>
  63.  
  64. #include <gazebo/math/Vector3.hh>
  65. #include <gazebo_plugins/GazeboRosRfidTag.h>
  66. #include <gazebo_plugins/GazeboRosRfid.h>
  67. #include <ros/ros.h>
  68.  
  69. namespace gazebo
  70. {
  71. // Register this plugin with the simulator
  72. GZ_REGISTER_MODEL_PLUGIN(GazeboRosRfid);
  73.  
  74. ////////////////////////////////////////////////////////////////////////////////
  75. // Constructor
  76. GazeboRosRfid::GazeboRosRfid() : SensorPlugin()
  77. {
  78. }
  79.  
  80. ////////////////////////////////////////////////////////////////////////////////
  81. // Destructor
  82. GazeboRosRfid::~GazeboRosRfid()
  83. {
  84.   this->rosnode_->shutdown();
  85.   this->callback_queue_thread_.join();
  86.  
  87.   delete this->rosnode_;
  88. }
  89.  
  90. ////////////////////////////////////////////////////////////////////////////////
  91. // Load the controller
  92. void GazeboRosRfid::Load( physics::ModelPtr _parent, sdf::ElementPtr _sdf )
  93. {
  94.   this->parentSensor = boost::dynamic_pointer_cast<sensors::RFIDSensor>(_parent);
  95.   if (!this->parentSensor)
  96.   {
  97.     ROS_ERROR("Rfid sensor parent is not of type RFIDSensor");
  98.     return;
  99.   }
  100.  
  101.   this->robot_namespace_ = "";
  102.   if (_sdf->HasElement("robotNamespace"))
  103.     this->robot_namespace_ =
  104.       _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";
  105.  
  106.   // "publishing contact/collisions to this topic name: "
  107.   // << this->bumper_topic_name_ << std::endl;
  108.   this->scan_topic_name_ = "rfidscan_poses";
  109.   if (_sdf->GetElement("scanTopicName"))
  110.     this->scan_topic_name_ =
  111.       _sdf->GetElement("scanTopicName")->Get<std::string>();
  112.  
  113.  
  114.  // "transform contact/collisions pose, forces to this body (link) name: "
  115.   // << this->frame_name_ << std::endl;
  116.   if (!_sdf->HasElement("frameName"))
  117.   {
  118.     ROS_INFO("scan plugin missing <frameName>, defaults to world");
  119.     this->frame_name_ = "world";
  120.   }
  121.   else
  122.     this->frame_name_ = _sdf->GetElement("frameName")->Get<std::string>();
  123.  
  124.   // Make sure the ROS node for Gazebo has already been initalized
  125.   if (!ros::isInitialized())
  126.   {
  127.     ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
  128.       << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
  129.     return;
  130.   }
  131.  
  132.  this->rosnode_ = new ros::NodeHandle(this->robot_namespace_);
  133.  
  134.  
  135.   // resolve tf prefix
  136.   std::string prefix;
  137.   this->rosnode_->getParam(std::string("tf_prefix"), prefix);
  138.   this->frame_name_ = tf::resolve(prefix, this->frame_name_);
  139.  
  140.   this->pose_pub_ = this->rosnode_->advertise<msgs::Pose>(
  141.     std::string(this->scan_topic_name_), 1);
  142.  
  143.   // Initialize
  144.   // start custom queue for contact bumper
  145.   this->callback_queue_thread_ = boost::thread(
  146.       boost::bind(&GazeboRosRfid::ScanQueueThread, this));
  147.  
  148.   // Listen to the update event. This event is broadcast every
  149.   // simulation iteration.
  150.   this->update_connection_ = this->parentSensor->ConnectUpdated(
  151.      boost::bind(&GazeboRosRfid::OnScan, this));
  152.  
  153.   // Make sure the parent sensor is active.
  154.   this->parentSensor->SetActive(true);
  155.  
  156.  
  157.  
  158.  
  159.  
  160.  
  161.  
  162.  
  163. }
  164.  
  165. ////////////////////////////////////////////////////////////////////////////////
  166. // Update the controller
  167. void GazeboRosRfid::OnScan()
  168. {
  169.   this->EvaluateTags();
  170.   this->lastMeasurementTime = this->world->GetSimTime();
  171.  
  172.   if (this->scanPub)
  173.   {
  174.     msgs::Pose msg;
  175.     msgs::Set(&msg, this->entity->GetWorldPose());
  176.     this->scanPub->Publish(msg);
  177.   }
  178.  
  179.   return;
  180. }
  181.  
  182. void GazeboRosRfid::EvaluateTags()
  183. {
  184.   std::vector<RFIDTag*>::const_iterator ci;
  185.  
  186.   // iterate through the tags contained given rfid tag manager
  187.   for (ci = this->tags.begin(); ci != this->tags.end(); ++ci)
  188.   {
  189.     math::Pose pos = (*ci)->GetTagPose();
  190.     // std::cout << "link: " << tagModelPtr->GetName() << std::endl;
  191.     // std::cout << "link pos: x" << pos.pos.x
  192.     //     << " y:" << pos.pos.y
  193.     //     << " z:" << pos.pos.z << std::endl;
  194.     this->CheckTagRange(pos);
  195.   }
  196. }
  197.  
  198. bool GazeboRosRfid::CheckTagRange(const math::Pose &_pose)
  199. {
  200.   // copy sensor vector pos into a temp var
  201.   math::Vector3 v;
  202.   v = _pose.pos - this->entity->GetWorldPose().pos;
  203.  
  204.   // std::cout << v.GetLength() << std::endl;
  205.  
  206.   if (v.GetLength() <= 5.0)
  207.   {
  208.     // std::cout << "detected " <<  v.GetLength() << std::endl;
  209.     return true;
  210.   }
  211.  
  212.   // this->CheckRayIntersection(link);
  213.   return false;
  214. }
  215.  
  216. void RFIDSensor::AddTag(RFIDTag *_tag)
  217. {
  218.   this->tags.push_back(_tag);
  219. }
  220.  
  221. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement