Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
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
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
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();
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
00130
00131
00132
00133
00134
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 }