| 271 | } |
| 272 | |
| 273 | void |
| 274 | SkeletonTrack::createMarker(visualization_msgs::MarkerArray::Ptr& msg, |
| 275 | bool remove_head_in_rviz) |
| 276 | { |
| 277 | // if(debug_count_ < 0 and SkeletonTrack::count == 0) |
| 278 | // ROS_WARN_STREAM("SkeletonTrack TODO: reqrite createMarker with tracks"); |
| 279 | |
| 280 | if(visibility_ == Track::NOT_VISIBLE) |
| 281 | return; |
| 282 | |
| 283 | std_msgs::ColorRGBA color_random, red; |
| 284 | color_random.a = red.a = 1.0; |
| 285 | color_random.r = color_(2); |
| 286 | color_random.g = color_(1); |
| 287 | color_random.b = color_(0); |
| 288 | red.r = 1.0; |
| 289 | red.b = red.g = 0.0; |
| 290 | |
| 291 | |
| 292 | |
| 293 | ros::Time now(ros::Time::now()); |
| 294 | |
| 295 | if (areJointsInitialized()) |
| 296 | { |
| 297 | // Joint Markers |
| 298 | for(int i = 0, end = SkeletonJoints::SIZE; i != end; ++i) |
| 299 | { |
| 300 | if(remove_head_in_rviz) |
| 301 | if(i == SkeletonJoints::HEAD) continue; |
| 302 | visualization_msgs::Marker joint_marker; |
| 303 | joint_marker.header.frame_id = frame_id_; |
| 304 | joint_marker.header.stamp = now; |
| 305 | joint_marker.ns = "joints_valid"; |
| 306 | joint_marker.id = i + id_ * SkeletonJoints::SIZE * 2; //for visualizing both detection and tracks |
| 307 | joint_marker.type = visualization_msgs::Marker::SPHERE; |
| 308 | joint_marker.action = visualization_msgs::Marker::ADD; |
| 309 | joint_marker.pose.position = joint_tracks_[i]->getState(); |
| 310 | joint_marker.pose.orientation.x = 0.0; |
| 311 | joint_marker.pose.orientation.y = 0.0; |
| 312 | joint_marker.pose.orientation.z = 0.0; |
| 313 | joint_marker.pose.orientation.w = 1.0; |
| 314 | joint_marker.scale.x = 0.06; |
| 315 | joint_marker.scale.y = 0.06; |
| 316 | joint_marker.scale.z = 0.06; |
| 317 | joint_marker.color.r = color_(2); |
| 318 | joint_marker.color.g = color_(1); |
| 319 | joint_marker.color.b = color_(0); |
| 320 | joint_marker.color.a = 1.0; |
| 321 | |
| 322 | joint_marker.lifetime = ros::Duration(0.2); |
| 323 | |
| 324 | |
| 325 | // geometry_msgs::Point p0; |
| 326 | // p0.x = p0.y = p0.z = 0; |
| 327 | // joint_marker.pose.position = p0; |
| 328 | msg->markers.push_back(joint_marker); |
| 329 | } |
| 330 | // Link markers |