32 #ifndef _RVIZ_MARKERS_HPP_
33 #define _RVIZ_MARKERS_HPP_
51 uint32_t shape = visualization_msgs::Marker::CUBE;
52 visualization_msgs::Marker marker;
53 marker.header.frame_id = frame_id;
54 marker.header.stamp = ros::Time::now();
56 marker.pose.position.x = 100;
57 marker.pose.position.y = 100;
58 marker.pose.position.z = 100;
59 marker.pose.orientation.x = 0.0;
60 marker.pose.orientation.y = 0.0;
61 marker.pose.orientation.z = 0.0;
62 marker.pose.orientation.w = 1.0;
68 marker.color.r = 1.0f;
69 marker.color.g = 0.0f;
70 marker.color.b = 0.0f;
76 marker.action = visualization_msgs::Marker::ADD;
77 marker.lifetime = ros::Duration();
83 visualization_msgs::Marker
markers<T>::mark(
typename PointCloud<T>::Ptr cloud_cluster,
string ns ,
int id,
float r,
float g,
float b)
85 Eigen::Vector4f centroid;
89 compute3DCentroid (*cloud_cluster, centroid);
90 getMinMax3D (*cloud_cluster, min, max);
92 uint32_t shape = visualization_msgs::Marker::CUBE;
93 visualization_msgs::Marker marker;
94 marker.header.frame_id = cloud_cluster->header.frame_id;
95 marker.header.stamp = ros::Time::now();
100 marker.action = visualization_msgs::Marker::ADD;
102 marker.pose.position.x = centroid[0];
103 marker.pose.position.y = centroid[1];
104 marker.pose.position.z = centroid[2];
105 marker.pose.orientation.x = 0.0;
106 marker.pose.orientation.y = 0.0;
107 marker.pose.orientation.z = 0.0;
108 marker.pose.orientation.w = 1.0;
110 marker.scale.x = (max[0]-min[0]);
111 marker.scale.y = (max[1]-min[1]);
112 marker.scale.z = (max[2]-min[2]);
114 if (marker.scale.x ==0)
117 if (marker.scale.y ==0)
120 if (marker.scale.z ==0)
126 marker.color.a = 0.3;
128 marker.lifetime = ros::Duration();
135 visualization_msgs::Marker
markers<T>::mark_text(
typename PointCloud<T>::Ptr cloud_cluster,
string ns ,
int id,
float r,
float g,
float b)
137 Eigen::Vector4f centroid;
141 compute3DCentroid (*cloud_cluster, centroid);
142 pcl::getMinMax3D (*cloud_cluster, min, max);
144 uint32_t shape = visualization_msgs::Marker::TEXT_VIEW_FACING;
145 visualization_msgs::Marker marker;
146 marker.header.frame_id = cloud_cluster->header.frame_id;
147 marker.header.stamp = ros::Time::now();
152 marker.action = visualization_msgs::Marker::ADD;
154 marker.pose.position.x = centroid[0];
155 marker.pose.position.y = centroid[1];
156 marker.pose.position.z = max[2]+0.2;
158 marker.scale.z = 0.4;
162 marker.color.a = 1.0;
168 marker.lifetime = ros::Duration();
visualization_msgs::Marker mark_text(typename PointCloud< T >::Ptr cloud_cluster, string ns, int id, float r, float g, float b)
function to create a marker with some text info about a pointcloud (cluster)
markers class main declaration
std::string to_string(const T &t)
convert numeric type to string This function should work for all numeric c++ types: int...
visualization_msgs::Marker unmark(std::string frame_id, std::string ns, int id)
function to estimate a marker arround the cluster to show on rviz This function will set a marker ver...
visualization_msgs::Marker mark(typename PointCloud< T >::Ptr cloud_cluster, string ns, int id, float r, float g, float b)
function to estimate a marker arround the cluster to show on rviz