grasp_marker_publisher.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 *
00003 *  Copyright (c) 2009, Willow Garage, Inc.
00004 *  All rights reserved.
00005 *
00006 *  Redistribution and use in source and binary forms, with or without
00007 *  modification, are permitted provided that the following conditions
00008 *  are met:
00009 *
00010 *   * Redistributions of source code must retain the above copyright
00011 *     notice, this list of conditions and the following disclaimer.
00012 *   * Redistributions in binary form must reproduce the above
00013 *     copyright notice, this list of conditions and the following
00014 *     disclaimer in the documentation and/or other materials provided
00015 *     with the distribution.
00016 *   * Neither the name of the Willow Garage nor the names of its
00017 *     contributors may be used to endorse or promote products derived
00018 *     from this software without specific prior written permission.
00019 *
00020 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00023 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00024 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00025 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00026 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00027 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00028 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00029 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00030 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031 *  POSSIBILITY OF SUCH DAMAGE.
00032 *********************************************************************/
00033 
00034 #include "object_manipulator/tools/grasp_marker_publisher.h"
00035 
00036 #include <string>
00037 #include <tf/transform_datatypes.h>
00038 
00039 namespace object_manipulator {
00040 
00041 static const std::string MARKERS_OUT_NAME = "grasp_execution_markers";
00042 static const std::string MARKERS_NAMESPACE = "grasp_markers";
00043 
00050 void GraspMarkerPublisher::init(std::string marker_out_name, std::string ns_append, double publishing_rate)
00051 {
00052   marker_pub_ = priv_nh_.advertise<visualization_msgs::Marker>(marker_out_name, 100);
00053   ns_append_ = ns_append;
00054   continuous_publishing_rate_ = publishing_rate;
00055   publishing_thread_ = NULL;
00056   if (continuous_publishing_rate_ > 0.0)
00057   {
00058     //ROS_INFO("Initializing publishing thread");
00059     publishing_thread_ = new boost::thread(boost::bind(&GraspMarkerPublisher::publishingThread, this)); 
00060   }
00061 }
00062 
00063 GraspMarkerPublisher::GraspMarkerPublisher() : 
00064   priv_nh_("~")
00065 {
00066   init(MARKERS_OUT_NAME, "", -1.0);
00067 }
00068 
00069 GraspMarkerPublisher::GraspMarkerPublisher(std::string marker_out_name, std::string ns_append, double publishing_rate) :
00070   priv_nh_("~")
00071 {
00072   init(marker_out_name, ns_append, publishing_rate);
00073 }
00074 
00075 GraspMarkerPublisher::~GraspMarkerPublisher()
00076 {
00077   if (publishing_thread_)
00078   {
00079     continuous_publishing_rate_ = -1.0;
00080     publishing_thread_->join();
00081     delete publishing_thread_;
00082   }
00083 }
00084 
00085 void GraspMarkerPublisher::publishingThread()
00086 {
00087   while (continuous_publishing_rate_ > 0.0)
00088   {
00089     ros::Duration(continuous_publishing_rate_).sleep();
00090     mutex_.lock();
00091     //ROS_INFO("Re-publishing markers");
00092     for (size_t g=0; g<grasp_markers_.size(); g++) 
00093     {
00094       grasp_markers_[g].header.stamp = ros::Time::now();
00095       marker_pub_.publish(grasp_markers_[g]);
00096     }  
00097     mutex_.unlock();
00098   }
00099 }
00100 
00101 void GraspMarkerPublisher::clearAllMarkers()
00102 {
00103   mutex_.lock();
00104   for (size_t g=0; g<grasp_markers_.size(); g++) 
00105   {
00106     grasp_markers_[g].action = visualization_msgs::Marker::DELETE;
00107     marker_pub_.publish(grasp_markers_[g]);
00108   }
00109   grasp_markers_.clear();
00110   mutex_.unlock();
00111 }
00112 
00113 unsigned int GraspMarkerPublisher::addGraspMarker(const geometry_msgs::PoseStamped &marker_pose)
00114 {
00115   visualization_msgs::Marker marker;
00116   marker.pose = marker_pose.pose;
00117   marker.header.frame_id = marker_pose.header.frame_id;
00118   marker.header.stamp = ros::Time::now();
00119   marker.ns = MARKERS_NAMESPACE + ns_append_;
00120   marker.action = visualization_msgs::Marker::ADD;
00121   marker.lifetime = ros::Duration();//ros::Duration(20);
00122   
00123   marker.type = visualization_msgs::Marker::ARROW;
00124   marker.scale.x = 0.1235;
00125   marker.scale.y = 0.06;
00126   marker.scale.z = 0.06;
00127   
00128   /*      
00129   marker.type = visualization_msgs::Marker::LINE_STRIP;
00130   marker.points.resize(2);
00131   marker.points[0].x = marker.points[0].y = marker.points[0].z = 0.0;
00132   marker.points[1].x = 0.1235;
00133   marker.points[1].y = marker.points[1].z = 0.0;
00134   marker.scale.x = 0.005;
00135   */
00136 
00137   marker.color.b = 1.0;
00138   marker.color.a = 1.0;
00139 
00140   mutex_.lock();
00141   marker.id = grasp_markers_.size();
00142   grasp_markers_.push_back(marker);
00143   marker_pub_.publish(marker);
00144   mutex_.unlock();
00145 
00146   return marker.id;
00147 }
00148 
00149 void GraspMarkerPublisher::colorGraspMarker(unsigned int marker_id, float r, float g, float b)
00150 {
00151   if (marker_id >= grasp_markers_.size()) 
00152   {
00153     ROS_WARN("Failed to change color of grasp marker %d", marker_id);
00154     return;
00155   }
00156 
00157   mutex_.lock();
00158   grasp_markers_[marker_id].color.r = r;
00159   grasp_markers_[marker_id].color.g = g;
00160   grasp_markers_[marker_id].color.b = b;
00161   grasp_markers_[marker_id].header.stamp = ros::Time::now();
00162   marker_pub_.publish(grasp_markers_[marker_id]);
00163   mutex_.unlock();
00164 
00165 }
00166 
00167 void GraspMarkerPublisher::setMarkerPose(unsigned int marker_id, const geometry_msgs::PoseStamped &marker_pose)
00168 {
00169   if (marker_id >= grasp_markers_.size()) 
00170   {
00171     ROS_WARN("Failed to change pose of grasp marker %d", marker_id);
00172     return;
00173   }
00174 
00175   mutex_.lock();
00176   grasp_markers_[marker_id].pose = marker_pose.pose;
00177   grasp_markers_[marker_id].header.frame_id = marker_pose.header.frame_id;
00178   grasp_markers_[marker_id].header.stamp = ros::Time::now();
00179   marker_pub_.publish(grasp_markers_[marker_id]);
00180   mutex_.unlock();
00181 
00182 }
00183 
00184 
00185 } //namespace object_manipulator


object_manipulator
Author(s): Matei Ciocarlie and Kaijen Hsiao
autogenerated on Thu Jan 2 2014 11:39:04