lmc_visualizer_node.cpp
Go to the documentation of this file.
1 #include "ros/ros.h"
2 #include "visualization_msgs/Marker.h"
3 #include "visualization_msgs/MarkerArray.h"
4 
5 #include "leap_motion/Human.h"
6 #include "leap_motion/Hand.h"
7 #include "leap_motion/Finger.h"
8 #include "leap_motion/Bone.h"
9 
10 #include "Leap.h"
11 #include <iostream>
12 
13 static ros::Publisher* marker_pub;
14 const std::string fingerNames[] = {"Thumb", "Index", "Middle", "Ring", "Pinky"};
15 
27 visualization_msgs::Marker createJointMarker(const leap_motion::Human::ConstPtr& human,
28  std::string hand_ns, std::string finger_ns, unsigned int id, geometry_msgs::Pose location ){
29 
30  visualization_msgs::Marker joint_marker;
31 
32  joint_marker.header.frame_id = human->header.frame_id;
33  joint_marker.header.stamp = human->header.stamp;
34  joint_marker.ns = hand_ns+"_"+finger_ns;
35  joint_marker.id = id;
36  joint_marker.type = visualization_msgs::Marker::SPHERE;
37  joint_marker.action = visualization_msgs::Marker::ADD;
38 
39  // Location data in meters from origin
40  joint_marker.pose.position.x = location.position.x;
41  joint_marker.pose.position.y = location.position.y;
42  joint_marker.pose.position.z = location.position.z;
43 
44  joint_marker.scale.x = 0.015;
45  joint_marker.scale.y = 0.015;
46  joint_marker.scale.z = 0.015;
47 
48  joint_marker.color.r = 1.0f;
49  joint_marker.color.g = 0.0f;
50  joint_marker.color.b = 0.0f;
51  joint_marker.color.a = 1.0f;
52  joint_marker.lifetime = ros::Duration(0.1);
53 
54  return joint_marker;
55 }
56 
68 visualization_msgs::Marker createFingerLines(const leap_motion::Human::ConstPtr &human,
69  std::string hand_ns, std::string finger_ns, unsigned int id, leap_motion::Bone current_bone){
70 
71  visualization_msgs::Marker line_list;
72  line_list.header.frame_id = human -> header.frame_id;
73  line_list.header.stamp = human -> header.stamp;
74  line_list.ns = hand_ns + "_" + finger_ns;
75  line_list.type = visualization_msgs::Marker::LINE_LIST;
76  line_list.action = visualization_msgs::Marker::ADD;
77  line_list.id = id;
78 
79  line_list.scale.x = 0.0075;
80 
81  // Line list is white
82  line_list.color.r = 1.0f;
83  line_list.color.g = 1.0f;
84  line_list.color.b = 1.0f;
85  line_list.color.a = 1.0f;
86  line_list.lifetime = ros::Duration(0.1);
87 
88  geometry_msgs::Point p;
89  p.x = current_bone.bone_start.position.x;
90  p.y = current_bone.bone_start.position.y;
91  p.z = current_bone.bone_start.position.z;
92  line_list.points.push_back(p);
93 
94  p.x = current_bone.bone_end.position.x;
95  p.y = current_bone.bone_end.position.y;
96  p.z = current_bone.bone_end.position.z;
97  line_list.points.push_back(p);
98 
99  return line_list;
100 }
101 
113 visualization_msgs::Marker createHandOutline(const leap_motion::Human::ConstPtr &human,
114  std::string hand_ns, unsigned int id, leap_motion::Hand current_hand){
115 
116  visualization_msgs::Marker line_list;
117  line_list.header.frame_id = human -> header.frame_id;
118  line_list.header.stamp = human -> header.stamp;
119  line_list.ns = hand_ns;
120  line_list.id = id;
121 
122  line_list.type = visualization_msgs::Marker::LINE_LIST;
123  line_list.action = visualization_msgs::Marker::ADD;
124 
125  line_list.scale.x = 0.0075;
126 
127  // Line list is white
128  line_list.color.r = 1.0f;
129  line_list.color.g = 1.0f;
130  line_list.color.b = 1.0f;
131  line_list.color.a = 1.0f;
132  line_list.lifetime = ros::Duration(0.1);
133 
134  for(unsigned int j = 0; j < current_hand.finger_list.size(); j++){
135 
136  geometry_msgs::Point p;
137 
138  leap_motion::Finger finger = current_hand.finger_list[j];
139  leap_motion::Bone bone = finger.bone_list[0];
140 
141  // Connects thumb start and index finger
142  if(finger.type == Leap::Finger::Type::TYPE_THUMB)
143  {
144  p.x = bone.bone_start.position.x;
145  p.y = bone.bone_start.position.y;
146  p.z = bone.bone_start.position.z;
147  line_list.points.push_back(p);
148 
149  // Assumption that the hand has all 5 fingers and
150  // that the fingers are given in order from thumb[idx 0] to pinky[idx 4].
151  finger = current_hand.finger_list[1];
152  bone = finger.bone_list[1];
153  p.x = bone.bone_start.position.x;
154  p.y = bone.bone_start.position.y;
155  p.z = bone.bone_start.position.z;
156  line_list.points.push_back(p);
157  }
158  // Connect wrist and pinky
159  else if(finger.type == Leap::Finger::Type::TYPE_PINKY)
160  {
161  p.x = bone.bone_start.position.x;
162  p.y = bone.bone_start.position.y;
163  p.z = bone.bone_start.position.z;
164  line_list.points.push_back(p);
165 
166  p.x = bone.bone_end.position.x;
167  p.y = bone.bone_end.position.y;
168  p.z = bone.bone_end.position.z;
169  line_list.points.push_back(p);
170 
171  // Creates the line from index to pinky at the wrist
172  p.x = bone.bone_start.position.x;
173  p.y = bone.bone_start.position.y;
174  p.z = bone.bone_start.position.z;
175  line_list.points.push_back(p);
176 
177  finger = current_hand.finger_list[0];
178  bone = finger.bone_list[0];
179  p.x = bone.bone_start.position.x;
180  p.y = bone.bone_start.position.y;
181  p.z = bone.bone_start.position.z;
182  line_list.points.push_back(p);
183  }
184  else
185  {
186  // Connects current finger and the follwing finger at the start of proximal bone (id 1)
187  leap_motion::Bone bone = finger.bone_list[1];
188  p.x = bone.bone_start.position.x;
189  p.y = bone.bone_start.position.y;
190  p.z = bone.bone_start.position.z;
191  line_list.points.push_back(p);
192 
193  leap_motion::Finger temp_finger = current_hand.finger_list[j+1];
194  bone = temp_finger.bone_list[1];
195  p.x = bone.bone_start.position.x;
196  p.y = bone.bone_start.position.y;
197  p.z = bone.bone_start.position.z;
198  line_list.points.push_back(p);
199  }
200  }
201  geometry_msgs::Point p;
202  p.x = current_hand.arm.elbow.position.x;
203  p.y = current_hand.arm.elbow.position.y;
204  p.z = current_hand.arm.elbow.position.z;
205  line_list.points.push_back(p);
206  p.x = current_hand.arm.wrist.position.x;
207  p.y = current_hand.arm.wrist.position.y;
208  p.z = current_hand.arm.wrist.position.z;
209  line_list.points.push_back(p);
210 
211  return line_list;
212 }
213 
225 visualization_msgs::Marker createPalmPosition(const leap_motion::Human::ConstPtr &human,
226  std::string hand_ns, unsigned int id, geometry_msgs::Point centre_point){
227 
228  visualization_msgs::Marker palm_centre;
229  palm_centre.header.frame_id = human -> header.frame_id;
230  palm_centre.header.stamp = human -> header.stamp;
231 
232  palm_centre.ns = hand_ns;
233  palm_centre.id = id;
234  palm_centre.type = visualization_msgs::Marker::SPHERE;
235  palm_centre.action = visualization_msgs::Marker::ADD;
236 
237  // Location data in meters from origin
238  palm_centre.pose.position.x = centre_point.x;
239  palm_centre.pose.position.y = centre_point.y;
240  palm_centre.pose.position.z = centre_point.z;
241 
242  palm_centre.scale.x = 0.025;
243  palm_centre.scale.y = 0.025;
244  palm_centre.scale.z = 0.025;
245 
246  palm_centre.color.r = 0.0f;
247  palm_centre.color.g = 0.0f;
248  palm_centre.color.b = 1.0f;
249  palm_centre.color.a = 1.0f;
250  palm_centre.lifetime = ros::Duration(0.1);
251 
252  return palm_centre;
253 }
254 
268 void createVisualization(visualization_msgs::MarkerArray* marker_array, const leap_motion::Human::ConstPtr& human,
269  leap_motion::Hand hand, std::string hand_ns, uint8_t hand_id){
270 
271  marker_array->markers.push_back(createPalmPosition(human, hand_ns, 55, hand.palm_center));
272  marker_array->markers.push_back(createHandOutline(human, hand_ns, hand_id, hand) );
273 
274  leap_motion::Finger finger;
275  for(unsigned int j = 0; j < hand.finger_list.size(); j++)
276  {
277  finger = hand.finger_list[j];
278  std::string ns_finger = fingerNames[finger.type];
279  unsigned int id_offset = finger.bone_list.size();
280 
281  leap_motion::Bone bone;
282  for(unsigned int k = 1; k < finger.bone_list.size(); k++)
283  {
284  bone = finger.bone_list[k];
285  marker_array->markers.push_back(createFingerLines(human, hand_ns, ns_finger, k, bone));
286  marker_array->markers.push_back(createJointMarker(human, hand_ns, ns_finger, k + id_offset, bone.bone_start));
287 
288  // The circle at the very bottom of the pinky
289  if(finger.type == Leap::Finger::Type::TYPE_PINKY)
290  {
291  leap_motion::Bone temp_bone = finger.bone_list[0];
292  marker_array->markers.push_back(createJointMarker(human, hand_ns, ns_finger,
293  k + id_offset+1, temp_bone.bone_start));
294  }
295  // Fingertip circles
296  if(k == Leap::Bone::Type::TYPE_DISTAL)
297  {
298  marker_array->markers.push_back(createJointMarker(human, hand_ns, ns_finger,
299  k + id_offset + 2, bone.bone_end));
300  }
301  }
302  }
303 }
304 
310 void frameCallback(const leap_motion::Human::ConstPtr& human){
311 
312  visualization_msgs::MarkerArray marker_array;
313  leap_motion::Hand hand;
314 
315  if( human -> right_hand.is_present )
316  {
317  hand = human -> right_hand;
318  createVisualization(&marker_array, human, hand, "Right", 0);
319  }
320 
321  if( human -> left_hand.is_present )
322  {
323  hand = human -> left_hand;
324  createVisualization(&marker_array, human, hand, "Left", 1);
325  }
326 
327  marker_pub->publish(marker_array);
328 }
329 
330 int main(int argc, char** argv) {
331  ros::init(argc, argv, "leap_motion");
332  ros::NodeHandle nh("leap_motion");
333  bool enable_filter;
334 
335  nh.getParam("/enable_filter", enable_filter);
336  ros::Subscriber human_sub;
337 
338  human_sub = nh.subscribe<leap_motion::Human>("leap_device", 1, frameCallback);
339  if(enable_filter)
340  {
341  human_sub = nh.subscribe<leap_motion::Human>("leap_filtered", 1, frameCallback);
342  }
343 
344  ROS_INFO("enable_filter: %s", enable_filter ? "true" : "false");
345 
346  ros::Publisher m_pub = nh.advertise<visualization_msgs::MarkerArray>("visualization_marker_array", 1);
347  marker_pub = &m_pub;
348 
349  ros::spin();
350  return 0;
351 }
352 
createJointMarker
visualization_msgs::Marker createJointMarker(const leap_motion::Human::ConstPtr &human, std::string hand_ns, std::string finger_ns, unsigned int id, geometry_msgs::Pose location)
Adds circles at the intersection of bones.
Definition: lmc_visualizer_node.cpp:27
marker_pub
static ros::Publisher * marker_pub
Definition: lmc_visualizer_node.cpp:13
ros::NodeHandle
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
createPalmPosition
visualization_msgs::Marker createPalmPosition(const leap_motion::Human::ConstPtr &human, std::string hand_ns, unsigned int id, geometry_msgs::Point centre_point)
Represents the midpoint of the palm.
Definition: lmc_visualizer_node.cpp:225
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
main
int main(int argc, char **argv)
Definition: lmc_visualizer_node.cpp:330
ros::spin
ROSCPP_DECL void spin(Spinner &spinner)
createFingerLines
visualization_msgs::Marker createFingerLines(const leap_motion::Human::ConstPtr &human, std::string hand_ns, std::string finger_ns, unsigned int id, leap_motion::Bone current_bone)
Adds lines between the tips of the bones.
Definition: lmc_visualizer_node.cpp:68
ROS_INFO
#define ROS_INFO(...)
frameCallback
void frameCallback(const leap_motion::Human::ConstPtr &human)
Called when a new Human.msg is received.
Definition: lmc_visualizer_node.cpp:310
fingerNames
const std::string fingerNames[]
Definition: lmc_visualizer_node.cpp:14
ros::NodeHandle::advertise
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
createVisualization
void createVisualization(visualization_msgs::MarkerArray *marker_array, const leap_motion::Human::ConstPtr &human, leap_motion::Hand hand, std::string hand_ns, uint8_t hand_id)
Creates a visualization of an entire hand from he wrist up.
Definition: lmc_visualizer_node.cpp:268
ros::NodeHandle::getParam
bool getParam(const std::string &key, std::string &s) const
createHandOutline
visualization_msgs::Marker createHandOutline(const leap_motion::Human::ConstPtr &human, std::string hand_ns, unsigned int id, leap_motion::Hand current_hand)
Creates a representation of palm.
Definition: lmc_visualizer_node.cpp:113

leap_motion
Author(s): Florian Lier , Mirza Shah , Isaac IY Saito
autogenerated on Tue Jun 2 2020 03:58:01